├── .gitignore ├── common └── usb_packet │ ├── usb_packet.cpp │ └── usb_packet.h ├── firmware ├── address_search │ ├── Makefile │ ├── address_search.ino │ └── pinouts.h ├── lib │ ├── bno055 │ │ ├── bno055.cpp │ │ └── bno055.h │ ├── i2c_t3 │ │ ├── README.md │ │ ├── examples │ │ │ ├── dual_bus_master_slave │ │ │ │ └── dual_bus_master_slave.ino │ │ │ ├── interrupt │ │ │ │ └── interrupt.ino │ │ │ ├── master │ │ │ │ └── master.ino │ │ │ ├── multiplexed_master │ │ │ │ └── multiplexed_master.ino │ │ │ ├── quad_master │ │ │ │ └── quad_master.ino │ │ │ ├── scanner │ │ │ │ └── scanner.ino │ │ │ ├── slave │ │ │ │ └── slave.ino │ │ │ ├── slave_range │ │ │ │ └── slave_range.ino │ │ │ └── timeout │ │ │ │ └── timeout.ino │ │ ├── i2c_t3.cpp │ │ ├── i2c_t3.h │ │ ├── keywords.txt │ │ └── speedtest.jpg │ └── i2c_wrapper │ │ ├── i2c_wrapper.cpp │ │ └── i2c_wrapper.h ├── multi_node_bringup │ ├── Makefile │ ├── multi_node_bringup.ino │ └── pinouts.h ├── single_node_bringup │ ├── Makefile │ ├── pinouts.h │ └── single_node_bringup.ino └── usb_node_device │ ├── Makefile │ └── usb_node_device.ino ├── pcbs ├── di2c_breakout │ ├── di2c_breakout.kicad_pcb │ ├── di2c_breakout.pro │ └── di2c_breakout.sch ├── imu_noodle_node_v1 │ ├── imu_noodle.kicad_pcb │ ├── imu_noodle.pro │ ├── imu_noodle.sch │ └── imu_noodle_rev-1.1_schematic_output.pdf ├── imu_noodle_node_v2 │ ├── imu_noodle.kicad_pcb │ ├── imu_noodle.pro │ ├── imu_noodle.sch │ └── imu_noodle_rev-2.0_schematic_output.pdf └── notes │ └── ltc4316_address_translator.ipynb └── visualization └── line_viz ├── Makefile ├── config.make └── src ├── main.cpp ├── node_usb_driver.cpp ├── node_usb_driver.h ├── ofApp.cpp ├── ofApp.h ├── ofxptf ├── ParallelTransportFrames.cpp ├── ParallelTransportFrames.h └── ofxPtf.h └── qpose └── src ├── commonMath.cpp ├── commonMath.hpp └── quaternion.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | *-cache.lib 2 | *.bak 3 | *.kicad_pcb-bak 4 | 5 | # Netlists 6 | *.net 7 | 8 | # Gerber formats 9 | *.gbl 10 | *.gbs 11 | *.gbp 12 | *.gbo 13 | *.gm1 14 | *.gtl 15 | *.gts 16 | *.gtp 17 | *.gto 18 | *.drl 19 | 20 | # Position files 21 | *.pos 22 | 23 | # zip files 24 | *.zip 25 | 26 | bin/ 27 | build-teensy31 28 | 29 | _autosave* 30 | 31 | obj/ 32 | -------------------------------------------------------------------------------- /common/usb_packet/usb_packet.cpp: -------------------------------------------------------------------------------- 1 | #include "usb_packet.h" 2 | 3 | USBPacket::USBPacket() 4 | {} 5 | 6 | 7 | USBPacket::~USBPacket() 8 | {} 9 | 10 | 11 | void USBPacket::getQuaternionPacket(size_t packetIndex, USBPacket::quatPacket& qPacket) 12 | { 13 | memcpy((void *) &qPacket, 14 | (void *) &packet_[packetIndex * sizeof(quatPacket)], 15 | sizeof(qPacket)); 16 | } 17 | 18 | 19 | void USBPacket::setQuaternionPacket(size_t packetIndex, USBPacket::quatPacket qPacket) 20 | { 21 | memcpy((void *) &packet_[packetIndex * sizeof(quatPacket)], 22 | (void *) &qPacket, 23 | sizeof(qPacket)); 24 | } 25 | 26 | -------------------------------------------------------------------------------- /common/usb_packet/usb_packet.h: -------------------------------------------------------------------------------- 1 | #ifndef USB_PACKET_H 2 | #define USB_PACKET_H 3 | #include 4 | #include 5 | #include 6 | 7 | /** 8 | * \brief a container for managing data transferred across USB. 9 | * This USB HID device transfers 64-byte packets, inside of which live 10 | * the quaternion data of several BNO055 sensors. 11 | * Which sensor's data is indicated by index field of the qPacket. 12 | */ 13 | class USBPacket 14 | { 15 | public: 16 | /* 17 | * \brief container for storing raw quaternion data. 18 | */ 19 | #pragma pack(push, 1) 20 | struct quatData 21 | { 22 | float w; 23 | float x; 24 | float y; 25 | float z; 26 | }; 27 | #pragma pack(pop) 28 | 29 | /** 30 | * \details container for storing both quaternion data and the index identifying 31 | * the corresponding sensor id. 32 | * \details this container is necessary for the usbPacket_ such that the 33 | * usbPacket_ may be stuffed with data from arbitrary sensors. 34 | */ 35 | #pragma pack(push, 1) 36 | struct quatPacket 37 | { 38 | uint8_t index; // which BNO055 in the chain we're talking to. 39 | quatData qData;// that BNO's data. 40 | }; 41 | #pragma pack(pop) 42 | 43 | 44 | USBPacket(); 45 | 46 | ~USBPacket(); 47 | 48 | /** 49 | * \brief get the quaternion data living at the packet index. 50 | * \param packetIndex which quatPacket slot in the packet_ that the 51 | * quatPacket will be inserted 52 | * \param qPacket a reference by which to read in the qPacket in the usb packet 53 | */ 54 | void getQuaternionPacket(size_t packetIndex, quatPacket& qPacket); 55 | 56 | /** 57 | * \brief set the quaternion data at a particular index in the packet. 58 | * \param packetIndex which quatPacket slot in the packet_ that the 59 | * quatPacket will be inserted 60 | * \param qPacket the qPacket to stuff into the USB packet 61 | */ 62 | void setQuaternionPacket(size_t packetIndex, quatPacket qPacket); 63 | 64 | 65 | static const size_t PACKET_SIZE_BYTES = 64; 66 | // Integer division is an implicit floor function. 67 | static const size_t QUATERNIONS_PER_PACKET = PACKET_SIZE_BYTES/sizeof(quatPacket); 68 | 69 | uint8_t packet_[PACKET_SIZE_BYTES]; 70 | }; 71 | 72 | 73 | #endif // USB_PACKET_H 74 | -------------------------------------------------------------------------------- /firmware/address_search/Makefile: -------------------------------------------------------------------------------- 1 | # Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile 2 | #The mobile embedded interface poses as a generic 3 | 4 | GIT_HASH = "\"$(shell git rev-parse HEAD)\"" 5 | GIT_BRANCH = "\"$(shell git branch | sed -n -e 's/^\* \(.*\)/\1/p')\"" 6 | UPLOAD_DATE = "\"$(shell date)\"" 7 | 8 | CXXFLAGS_STD = -std=gnu++11 -DGIT_HASH=$(GIT_HASH) -DGIT_BRANCH=$(GIT_BRANCH) \ 9 | -DUPLOAD_DATE=$(UPLOAD_DATE) 10 | 11 | USB_TYPE = USB_SERIAL 12 | 13 | # WARNING: Trailing whitespace will cause options to be read incorrectly. 14 | BOARD_TAG = teensy31 15 | 16 | # Additional user-defined libraries may be added to this list 17 | ARDUINO_LIBS += bno055 \ 18 | i2c_wrapper \ 19 | i2c_t3 20 | 21 | # Default user-defined libraries directory 22 | USER_LIB_PATH += ../lib 23 | 24 | # Teensy explicitly requires Arduino 1.0.x as of Nov 2014 25 | # ARDUINO_DIR should be defined in your Makefile. Otherwise, you may 26 | # define it here. 27 | 28 | # teensy3, teensy31, and teensy32 explicitly require their own Makefile. 29 | include $(ARDMK_DIR)/Teensy.mk 30 | 31 | -------------------------------------------------------------------------------- /firmware/address_search/address_search.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * \project single_board_bringup.ino 3 | * \author Joshua Vasquez 4 | */ 5 | 6 | #include "pinouts.h" 7 | #include 8 | #include 9 | 10 | BNO055 bno055; 11 | 12 | void setup(){ 13 | Serial.begin(9600);// baud rate irrelevant for usb serial. 14 | delay(3000); 15 | Serial.print("Git hash: "); 16 | Serial.println(GIT_HASH); 17 | Serial.print("Git branch: "); 18 | Serial.println(GIT_BRANCH); 19 | Serial.print("Upload Date: "); 20 | Serial.println(UPLOAD_DATE); 21 | i2cInit(); 22 | delay(1000); 23 | for (uint8_t address = 0x1F; address < 120; ++address) 24 | { 25 | if(!bno055.init(BNO055::NDOF, (BNO055::address_t)address)) 26 | { 27 | Serial.print("BNO055 at "); 28 | Serial.print("0x"); 29 | Serial.print(address, HEX); 30 | Serial.println(" init succeeded."); 31 | break; 32 | } 33 | else 34 | { 35 | Serial.print("BNO055 at "); 36 | Serial.print("0x"); 37 | Serial.print(address, HEX); 38 | Serial.println(" init failed."); 39 | } 40 | } 41 | } 42 | 43 | void loop(void) 44 | { 45 | } 46 | -------------------------------------------------------------------------------- /firmware/address_search/pinouts.h: -------------------------------------------------------------------------------- 1 | #ifndef PINOUTS_H 2 | #define PINOUTS_H 3 | 4 | 5 | #endif // PINOUTS_H 6 | -------------------------------------------------------------------------------- /firmware/lib/bno055/bno055.cpp: -------------------------------------------------------------------------------- 1 | #include "bno055.h" 2 | 3 | BNO055::BNO055(): 4 | deviceAddress_(DEFAULT_ADDRESS), 5 | currentMode_(CONFIGMODE) 6 | { 7 | } 8 | 9 | BNO055::~BNO055() 10 | { 11 | } 12 | 13 | uint8_t BNO055::init(mode_t mode, address_t device_address) 14 | { 15 | uint8_t status_byte = 0; 16 | deviceAddress_ = device_address; 17 | /// Delay for the Power-On-Reset Time. 18 | delay(POWER_ON_RESET_TIME_MS); 19 | status_byte = setGyroUnits(RADIANS_PER_SECOND); 20 | if (status_byte) {return status_byte;} 21 | status_byte = setAccelUnits(METERS_PER_SQUARE_SECOND); 22 | if (status_byte) {return status_byte;} 23 | status_byte = setEulerAngleUnits(RADIANS); 24 | if (status_byte) {return status_byte;} 25 | status_byte = setTemperatureUnits(CELSIUS); 26 | if (status_byte) {return status_byte;} 27 | /// Default power-up mode is CONFIGMODE; change here. 28 | status_byte = setMode(mode); 29 | return status_byte; 30 | } 31 | 32 | uint8_t BNO055::readRawData(uint8_t * const arrayPtr) 33 | { 34 | return i2cRead(deviceAddress_, ACC_DATA_X_LSB, RAW_DATA_NUM_BYTES, 35 | arrayPtr); 36 | } 37 | 38 | uint8_t BNO055::readCalibData(uint8_t * const arrayPtr) 39 | { 40 | return i2cRead(deviceAddress_, ACC_OFFSET_X_LSB, CALIB_DATA_NUM_BYTES, 41 | arrayPtr); 42 | } 43 | 44 | uint8_t BNO055::writeCalibData(uint8_t * const arrayPtr) 45 | { 46 | return i2cWrite(deviceAddress_, ACC_OFFSET_X_LSB, CALIB_DATA_NUM_BYTES, 47 | arrayPtr); 48 | } 49 | 50 | uint8_t BNO055::readCalibrationStatus(void) 51 | { 52 | uint8_t value; 53 | i2cReadByte(deviceAddress_, CALIB_STAT, value); 54 | return value; 55 | } 56 | 57 | bool BNO055::isDeviceCalibrated(void) 58 | { 59 | const uint8_t FULLY_CALIBRATED = 3; 60 | const uint8_t SYS_MASK = 0xC0; 61 | uint8_t calibrationStatus = (readCalibrationStatus() & SYS_MASK) >> 6; 62 | return (calibrationStatus == FULLY_CALIBRATED); 63 | } 64 | 65 | bool BNO055::isSensorCalibrated(sensor_t sensor) 66 | { 67 | const uint8_t FULLY_CALIBRATED = 3; 68 | 69 | const uint8_t MAG_CALIB_STATUS_MASK = 0x03; 70 | const uint8_t MAG_CALIB_STATUS_BIT_OFFSET = 0; 71 | const uint8_t ACCEL_CALIB_STATUS_MASK = 0x0C; 72 | const uint8_t ACCEL_CALIB_STATUS_BIT_OFFSET = 2; 73 | const uint8_t GYRO_CALIB_STATUS_MASK = 0x30; 74 | const uint8_t GYRO_CALIB_STATUS_BIT_OFFSET = 4; 75 | 76 | uint8_t mask = 0; 77 | uint8_t bitOffset = 0; 78 | if (sensor == ACCELEROMETER) 79 | { 80 | mask = ACCEL_CALIB_STATUS_MASK; 81 | bitOffset = ACCEL_CALIB_STATUS_BIT_OFFSET; 82 | } 83 | if (sensor == MAGNETOMETER) 84 | { 85 | mask = MAG_CALIB_STATUS_MASK; 86 | bitOffset = MAG_CALIB_STATUS_BIT_OFFSET; 87 | } 88 | if (sensor == GYROSCOPE) 89 | { 90 | mask = GYRO_CALIB_STATUS_MASK; 91 | bitOffset = GYRO_CALIB_STATUS_BIT_OFFSET; 92 | } 93 | return (((readCalibrationStatus() & mask) >> bitOffset) == FULLY_CALIBRATED); 94 | } 95 | 96 | uint16_t BNO055::read16(uint8_t starting_address) 97 | { 98 | const uint8_t num_bytes = 2; 99 | uint8_t temp_array[num_bytes]; 100 | i2cRead(deviceAddress_, starting_address, num_bytes, temp_array); 101 | return (int16_t(temp_array[1]) << 8) | int16_t(temp_array[0]); 102 | } 103 | 104 | uint16_t BNO055::readRawAccel(axis_t axis) 105 | { 106 | uint8_t baseAddress = ACC_DATA_X_LSB; 107 | // Register addresses follow the pattern X_LSB, X_MSB, ... Z_LSB, Z_MSB, 108 | // so incrementing by 2 will access the next axis 109 | return read16(baseAddress + (2 * axis)); 110 | } 111 | 112 | uint16_t BNO055::readRawMag(axis_t axis) 113 | { 114 | uint8_t baseAddress = MAG_DATA_X_LSB; 115 | // Register addresses follow the pattern X_LSB, X_MSB, ... Z_LSB, Z_MSB, 116 | // so incrementing by 2 will access the next axis 117 | return read16(baseAddress + (2 * axis)); 118 | } 119 | 120 | uint16_t BNO055::readRawGyro(axis_t axis) 121 | { 122 | uint8_t baseAddress = GYR_DATA_X_LSB; 123 | // Register addresses follow the pattern X_LSB, X_MSB, ... Z_LSB, Z_MSB, 124 | // so incrementing by 2 will access the next axis 125 | return read16(baseAddress + (2 * axis)); 126 | } 127 | 128 | uint16_t BNO055::readEulHead(void) 129 | { 130 | return read16(EUL_HEADING_LSB); 131 | } 132 | 133 | uint16_t BNO055::readLinearAccel(axis_t axis) 134 | { 135 | uint8_t baseAddress = LIA_DATA_X_LSB; 136 | // Register addresses follow the pattern X_LSB, X_MSB, ... Z_LSB, Z_MSB, 137 | // so incrementing by 2 will access the next axis 138 | return read16(baseAddress + (2 * axis)); 139 | } 140 | 141 | uint16_t BNO055::readGravityVector(axis_t axis) 142 | { 143 | uint8_t baseAddress = GRV_DATA_X_LSB; 144 | // Register addresses follow the pattern X_LSB, X_MSB, ... Z_LSB, Z_MSB, 145 | // so incrementing by 2 will access the next axis 146 | return read16(baseAddress + (2 * axis)); 147 | } 148 | 149 | uint8_t BNO055::readRawQuaternion(int16_t& w, int16_t& x, int16_t& y, int16_t& z) 150 | { 151 | uint8_t quaternionNumBytes = 8; 152 | uint8_t quaternionData[quaternionNumBytes]; 153 | uint8_t failure = i2cRead(deviceAddress_, QUA_DATA_W_LSB, quaternionNumBytes, 154 | quaternionData); 155 | if (failure) 156 | return failure; 157 | w = (((int16_t)quaternionData[1]) << 8) | quaternionData[0]; 158 | x = (((int16_t)quaternionData[3]) << 8) | quaternionData[2]; 159 | y = (((int16_t)quaternionData[5]) << 8) | quaternionData[4]; 160 | z = (((int16_t)quaternionData[7]) << 8) | quaternionData[6]; 161 | return 0; 162 | } 163 | 164 | BNO055::mode_t BNO055::getMode() 165 | { 166 | return currentMode_; 167 | } 168 | 169 | uint8_t BNO055::setMode(mode_t mode) 170 | { 171 | /// status_byte will be 0 iff all transactions succeed. 172 | uint8_t status_byte = 0; 173 | mode_t previous_mode = getMode(); 174 | /// Read 175 | uint8_t opr_mode_bits; 176 | status_byte = i2cReadByte(deviceAddress_, OPR_MODE, opr_mode_bits); 177 | if (status_byte) {return status_byte;} 178 | /// Modify. (Clear bits 6:4) 179 | opr_mode_bits &= ~(OPR_MODE_MASK); 180 | /// Set new mode bits 181 | opr_mode_bits |= (mode << OPR_MODE_OFFSET); 182 | // Write back. 183 | status_byte = i2cWriteByte(deviceAddress_, OPR_MODE, opr_mode_bits); 184 | if (status_byte) {return status_byte;} 185 | 186 | /// Remember mode for other operations that need it. 187 | currentMode_ = mode; 188 | 189 | /// Delay time depends on the previous mode. 190 | /// Magic values from pg 21 of the datasheet. 191 | if (previous_mode == CONFIGMODE) 192 | delay(7); 193 | else 194 | delay(19); 195 | 196 | /// TODO: add readback to ensure that setMode worked. 197 | return 0; 198 | } 199 | 200 | uint8_t BNO055::setGyroUnits(gyro_units_t units) 201 | { 202 | /// status_byte will be 0 iff all transactions succeed. 203 | uint8_t status_byte = 0; 204 | mode_t previous_mode = getMode(); 205 | status_byte = setMode(CONFIGMODE); 206 | if (status_byte) {return status_byte;} 207 | /// Read. 208 | uint8_t unit_sel_bits; 209 | status_byte = i2cReadByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 210 | if (status_byte) {return status_byte;} 211 | /// Modify. (clear units and then set the desired units) 212 | unit_sel_bits &= ~(1 << GYR_UNIT); 213 | unit_sel_bits |= (units << GYR_UNIT); 214 | // Write back. 215 | status_byte = i2cWriteByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 216 | if (status_byte) {return status_byte;} 217 | status_byte = setMode(previous_mode); 218 | if (status_byte) {return status_byte;} 219 | 220 | /// TODO: add readback to ensure that setMode worked. 221 | return 0; 222 | } 223 | 224 | uint8_t BNO055::setAccelUnits(accel_units_t units) 225 | { 226 | /// status_byte will be 0 iff all transactions succeeded. 227 | uint8_t status_byte = 0; 228 | mode_t previous_mode = getMode(); 229 | status_byte = setMode(CONFIGMODE); 230 | if (status_byte) {return status_byte;} 231 | /// Read. 232 | uint8_t unit_sel_bits; 233 | status_byte = i2cReadByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 234 | if (status_byte) {return status_byte;} 235 | /// Modify. (Clear ACC_UNIT bit.) 236 | unit_sel_bits &= ~(1 << ACC_UNIT); 237 | /// Set new unit 238 | unit_sel_bits |= (units << ACC_UNIT); 239 | // Write back. 240 | status_byte = i2cWriteByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 241 | if (status_byte) {return status_byte;} 242 | 243 | status_byte = setMode(previous_mode); 244 | if (status_byte) {return status_byte;} 245 | 246 | /// TODO: add readback to ensure that setMode worked. 247 | return 0; 248 | } 249 | 250 | uint8_t BNO055::setEulerAngleUnits(euler_angle_units_t units) 251 | { 252 | /// Status byte will be 0 iff all transactions succeed. 253 | uint8_t status_byte = 0; 254 | mode_t previous_mode = getMode(); 255 | status_byte = setMode(CONFIGMODE); 256 | if (status_byte) {return status_byte;} 257 | /// Read. 258 | uint8_t unit_sel_bits; 259 | status_byte = i2cReadByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 260 | if (status_byte) {return status_byte;} 261 | /// Modify. (Clear EUL_UNIT bit.) 262 | unit_sel_bits &= ~(1 << EUL_UNIT); 263 | /// Set new unit 264 | unit_sel_bits |= (units << EUL_UNIT); 265 | // Write back. 266 | status_byte = i2cWriteByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 267 | if (status_byte) {return status_byte;} 268 | 269 | status_byte = setMode(previous_mode); 270 | if (status_byte) {return status_byte;} 271 | 272 | /// TODO: add readback to ensure that setMode worked. 273 | return 0; 274 | } 275 | 276 | uint8_t BNO055::setTemperatureUnits(temperature_units_t units) 277 | { 278 | /// Status byte will be 0 iff all transactions succeed. 279 | uint8_t status_byte = 0; 280 | mode_t previous_mode = getMode(); 281 | status_byte = setMode(CONFIGMODE); 282 | if (status_byte) {return status_byte;} 283 | /// Read. 284 | uint8_t unit_sel_bits; 285 | status_byte = i2cReadByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 286 | if (status_byte) {return status_byte;} 287 | /// Modify. (Clear TEMP_UNIT bit.) 288 | unit_sel_bits &= ~(1 << TEMP_UNIT); 289 | /// Set new unit 290 | unit_sel_bits |= (units << TEMP_UNIT); 291 | // Write back. 292 | status_byte = i2cWriteByte(deviceAddress_, UNIT_SEL, unit_sel_bits); 293 | if (status_byte) {return status_byte;} 294 | 295 | status_byte = setMode(previous_mode); 296 | if (status_byte) {return status_byte;} 297 | 298 | /// TODO: add readback to ensure that setMode worked. 299 | return 0; 300 | } 301 | 302 | uint8_t BNO055::axisRemap(int8_t new_sign_x, axis_t new_x, 303 | int8_t new_sign_y, axis_t new_y, 304 | int8_t new_sign_z, axis_t new_z) 305 | { 306 | /// Status byte will be 0 iff all transactions succeed. 307 | uint8_t status_byte = 0; 308 | /// Save the previous mode. 309 | mode_t previous_mode = getMode(); 310 | status_byte = setMode(CONFIGMODE); 311 | if (status_byte) {return status_byte;} 312 | 313 | /// Get old value. 314 | uint8_t axis_bits; 315 | status_byte = i2cReadByte(deviceAddress_, AXIS_MAP_CONFIG, axis_bits); 316 | if (status_byte) {return status_byte;} 317 | /// Remove old values (two bits each) and apply changes. 318 | axis_bits &= ~( (AXIS_MASK << REMAPPED_X_OFFSET) 319 | | (AXIS_MASK << REMAPPED_Y_OFFSET) 320 | | (AXIS_MASK << REMAPPED_Z_OFFSET)); 321 | /// Add new values: 322 | axis_bits |= (new_x << REMAPPED_X_OFFSET) 323 | | (new_y << REMAPPED_Y_OFFSET) 324 | | (new_z << REMAPPED_Z_OFFSET); 325 | /// Write back new settings. 326 | status_byte = i2cWriteByte(deviceAddress_, AXIS_MAP_CONFIG, axis_bits); 327 | if (status_byte) {return status_byte;} 328 | 329 | /// Get old value. 330 | uint8_t axis_sign_bits; 331 | status_byte = i2cReadByte(deviceAddress_, AXIS_MAP_SIGN, axis_sign_bits); 332 | if (status_byte) {return status_byte;} 333 | /// Remove old values and apply changes. 334 | axis_sign_bits &= ~( (1 << REMAPPED_X_SIGN_OFFSET) 335 | | (1 << REMAPPED_Y_SIGN_OFFSET) 336 | | (1 << REMAPPED_Z_SIGN_OFFSET)); 337 | /// Add new values: 338 | uint8_t sign_x = (new_sign_x < 0) ? 1 : 0; 339 | uint8_t sign_y = (new_sign_y < 0) ? 1 : 0; 340 | uint8_t sign_z = (new_sign_z < 0) ? 1 : 0; 341 | 342 | axis_sign_bits |= (sign_x << REMAPPED_X_SIGN_OFFSET) 343 | | (sign_y << REMAPPED_Y_SIGN_OFFSET) 344 | | (sign_z << REMAPPED_Z_SIGN_OFFSET); 345 | 346 | /// Write back new settings. 347 | status_byte = i2cWriteByte(deviceAddress_, AXIS_MAP_SIGN, axis_sign_bits); 348 | if (status_byte) {return status_byte;} 349 | 350 | /// Put back the previous mode. 351 | status_byte = setMode(previous_mode); 352 | if (status_byte) {return status_byte;} 353 | return 0; 354 | } 355 | 356 | -------------------------------------------------------------------------------- /firmware/lib/bno055/bno055.h: -------------------------------------------------------------------------------- 1 | #ifndef BNO055_H_ 2 | #define BNO055_H_ 3 | #include 4 | #include 5 | #include 6 | 7 | /** 8 | * \brief a lightweight BNO055 driver 9 | * \details this driver may be generalized to another 10 | * microcontroller by simply rewriting the i2c wrapper 11 | * implementation and implementing a delay function. 12 | */ 13 | class BNO055 14 | { 15 | public: 16 | enum address_t 17 | { 18 | DEFAULT_ADDRESS = 0x29, /// Device address if COM3 pin is Logic-1 19 | ALTERNATE_ADDRESS = 0x28 /// Device address if COM3 pin is Logic-0 20 | }; 21 | 22 | enum sensor_t 23 | { 24 | ACCELEROMETER = 0, 25 | GYROSCOPE = 1, 26 | MAGNETOMETER = 2 27 | }; 28 | 29 | enum gyro_units_t 30 | { 31 | DEGREES_PER_SECOND = 0, 32 | RADIANS_PER_SECOND = 1 33 | }; 34 | 35 | enum accel_units_t 36 | { 37 | METERS_PER_SQUARE_SECOND = 0, 38 | MILLI_GS = 1 39 | }; 40 | 41 | enum temperature_units_t 42 | { 43 | CELSIUS = 0, 44 | FAHRENHEIT= 1 45 | }; 46 | 47 | enum euler_angle_units_t { 48 | DEGREES = 0, 49 | RADIANS = 1 50 | }; 51 | 52 | enum mode_t 53 | { 54 | CONFIGMODE = 0x00, 55 | ACCONLY = 0x01, 56 | MAGONLY = 0x02, 57 | GYROONLY = 0x03, 58 | ACCMAG = 0x04, /// Accelerometer and magnetometer turned on 59 | ACCGYRO = 0x05, /// Accelerometer and Gyro turned on 60 | MAGGYRO = 0x06, /// Magnetometer and Gyro turned on 61 | AMG = 0x07, /// All 3 sensors turned on 62 | IMU = 0x08, /// Fusion mode w/ Acc and Gyro 63 | COMPASS = 0x09, /// Fusion mode w/ Heading data only 64 | M4G = 0x0A, /// Fusion mode w/ Magnet-for-gyroscope 65 | NDOF_FMC_OFF = 0x0B, /// Fusion mode w/o magnetometer auto-calibration 66 | NDOF = 0x0C /// Fusion mode w/ magnetometer auto-calibration 67 | }; 68 | 69 | enum axis_t 70 | { 71 | X_AXIS = 0, 72 | Y_AXIS = 1, 73 | Z_AXIS = 2 74 | }; 75 | 76 | BNO055(); 77 | ~BNO055(); 78 | 79 | /** 80 | * \brief initialize the bno055 device so that communication can begin with 81 | * the sensor. 82 | * \param device_address i2c address 83 | * \return a status byte, where 0 indicates success and 1 indicates failure 84 | */ 85 | uint8_t init(mode_t mode = NDOF, address_t device_address = DEFAULT_ADDRESS); 86 | 87 | /** 88 | * \brief perform a data dump of registers 0x08 through 0x35 89 | * into the specified array starting at the index location. 90 | * \param arrayPtr the name of the array where data will be written 91 | */ 92 | uint8_t readRawData(uint8_t * const arrayPtr); 93 | 94 | /** 95 | * \brief perform a data dump of registers 0x55 through 0x6A 96 | * into the specified array starting at the index location. 97 | * \param arrayPtr the name of the array where data will be written 98 | */ 99 | uint8_t readCalibData(uint8_t * const arrayPtr); 100 | 101 | /** 102 | * \brief write back calibration data to the calibration registers 103 | * \param arrayPtr the name of the array containing data to be written 104 | */ 105 | uint8_t writeCalibData(uint8_t * const arrayPtr); 106 | 107 | /** 108 | * \brief read the register for calibration status 109 | */ 110 | uint8_t readCalibrationStatus(void); 111 | 112 | /** 113 | * \brief return the device calibration status 114 | */ 115 | uint8_t readDeviceCalibration(void); 116 | 117 | /** 118 | * \brief quickly check if the BNO055 is calibrated 119 | */ 120 | bool isDeviceCalibrated(void); 121 | 122 | /** 123 | * \brief check if the sensor in question is calibrated 124 | */ 125 | bool isSensorCalibrated(sensor_t sensor); 126 | 127 | /** 128 | * \brief grab the raw accelerometer data for a given axis 129 | * \param axis the axis_t enumeration corresponding to the desired axis 130 | */ 131 | uint16_t readRawAccel(axis_t axis); 132 | 133 | /** 134 | * \brief grab the raw magnetometer data for a given axis 135 | * \param axis the axis_t enumeration corresponding to the desired axis 136 | */ 137 | uint16_t readRawMag(axis_t axis); 138 | 139 | /** 140 | * \brief grab the raw gyroscope data for a given axis 141 | * \param axis the axis_t enumeration corresponding to the desired axis 142 | */ 143 | uint16_t readRawGyro(axis_t axis); 144 | 145 | /** 146 | * \brief read the Euler heading from the sensor fusion algorithm 147 | */ 148 | uint16_t readEulHead(void); 149 | 150 | /** 151 | * \brief read the linear acceleration data for a given axis (sensor fusion) 152 | * \param axis the axis_t enumeration corresponding to the desired axis 153 | */ 154 | uint16_t readLinearAccel(axis_t axis); 155 | 156 | /** 157 | * \brief read the gravitational vector data for a given axis (sensor fusion) 158 | * \param axis the axis_t enumeration corresponding to the desired axis 159 | */ 160 | uint16_t readGravityVector(axis_t axis); 161 | 162 | /** 163 | * \brief Get the unscaled quaternion data. Return i2c status byte, 164 | where nonzero indicates failure. 165 | */ 166 | uint8_t readRawQuaternion(int16_t& w, int16_t& x, int16_t& y, int16_t& z); 167 | 168 | /** 169 | * \brief get the mode from one of the various available modes. 170 | */ 171 | mode_t getMode(); 172 | /** 173 | * \brief set the mode from one of the various available modes. 174 | */ 175 | uint8_t setMode(mode_t m); 176 | 177 | /** 178 | * \brief set the units to radians and radians-per-second. 179 | */ 180 | uint8_t setGyroUnits(gyro_units_t units); 181 | 182 | /** 183 | * \brief set accel units to either mg or m/(s^2) 184 | */ 185 | uint8_t setAccelUnits(accel_units_t units); 186 | 187 | /** 188 | * \brief set euler angle units to either mg or m/(s^2) 189 | */ 190 | uint8_t setEulerAngleUnits(euler_angle_units_t units); 191 | 192 | /** 193 | * \brief set the units of the temperature 194 | */ 195 | uint8_t setTemperatureUnits(temperature_units_t units); 196 | 197 | /** 198 | * \brief remap axis to account for alternate mounting orientation. 199 | * \details new orientation is relative to the default initial orientation. 200 | * On-chip mode is switched to CONNFIGMODE and then restored before 201 | function returns. 202 | * \param new_sign_x +/- value to determine new sign of the new x axis 203 | * \param new_x axis specifying the new x-axis 204 | * \param new_sign_y +/- value to determine new sign of the new y axis 205 | * \param new_y axis specifying the new y-axis 206 | * \param new_sign_z +/- value to determine new sign of the new z axis 207 | * \param new_z axis specifying the new z-axis 208 | */ 209 | uint8_t axisRemap(int8_t new_sign_x, axis_t new_x, 210 | int8_t new_sign_y, axis_t new_y, 211 | int8_t new_sign_z, axis_t new_z); 212 | 213 | static const uint8_t RAW_DATA_NUM_BYTES = 46; 214 | static const uint8_t CALIB_DATA_NUM_BYTES = 22; 215 | static const uint32_t QUAT_BITS_TO_FLOAT = 16384; 216 | 217 | private: 218 | 219 | /** 220 | * \brief read two subsequent bytes by the address of the least significant byte 221 | * and return their combined value as a 16-bit integer 222 | * \param starting_addresss the starting address to read from. 223 | In the BNO055 register mapping, it is the least significant byte. 224 | */ 225 | uint16_t read16(uint8_t starting_address); 226 | 227 | /// The current device address according to the wiring. 228 | address_t deviceAddress_; 229 | mode_t currentMode_; 230 | 231 | /// Constants 232 | static const uint8_t ACC_DATA_X_LSB = 0x08; 233 | static const uint8_t ACC_DATA_X_MSB = 0x09; 234 | static const uint8_t ACC_DATA_Y_LSB = 0x0A; 235 | static const uint8_t ACC_DATA_Y_MSB = 0x0B; 236 | static const uint8_t ACC_DATA_Z_LSB = 0x0C; 237 | static const uint8_t ACC_DATA_Z_MSB = 0x0D; 238 | 239 | static const uint8_t MAG_DATA_X_LSB = 0x0E; 240 | static const uint8_t MAG_DATA_X_MSB = 0x0F; 241 | static const uint8_t MAG_DATA_Y_LSB = 0x10; 242 | static const uint8_t MAG_DATA_Y_MSB = 0x11; 243 | static const uint8_t MAG_DATA_Z_LSB = 0x12; 244 | static const uint8_t MAG_DATA_Z_MSB = 0x13; 245 | 246 | static const uint8_t GYR_DATA_X_LSB = 0x14; 247 | static const uint8_t GYR_DATA_X_MSB = 0x15; 248 | static const uint8_t GYR_DATA_Y_LSB = 0x16; 249 | static const uint8_t GYR_DATA_Y_MSB = 0x17; 250 | static const uint8_t GYR_DATA_Z_LSB = 0x18; 251 | static const uint8_t GYR_DATA_Z_MSB = 0x19; 252 | 253 | static const uint8_t EUL_HEADING_LSB = 0x1A; 254 | static const uint8_t EUL_HEADING_MSB = 0x1B; 255 | static const uint8_t EUL_ROLL_LSB = 0x1C; 256 | static const uint8_t EUL_ROLL_MSB = 0x1D; 257 | static const uint8_t EUL_PITCH_LSB = 0x1E; 258 | static const uint8_t EUL_PITCH_MSB = 0x1F; 259 | 260 | static const uint8_t QUA_DATA_W_LSB = 0x20; 261 | static const uint8_t QUA_DATA_W_MSB = 0x21; 262 | static const uint8_t QUA_DATA_X_LSB = 0x22; 263 | static const uint8_t QUA_DATA_X_MSB = 0x23; 264 | static const uint8_t QUA_DATA_Y_LSB = 0x24; 265 | static const uint8_t QUA_DATA_Y_MSB = 0x25; 266 | static const uint8_t QUA_DATA_Z_LSB = 0x26; 267 | static const uint8_t QUA_DATA_Z_MSB = 0x27; 268 | 269 | static const uint8_t LIA_DATA_X_LSB = 0x28; 270 | static const uint8_t LIA_DATA_X_MSB = 0x29; 271 | static const uint8_t LIA_DATA_Y_LSB = 0x2A; 272 | static const uint8_t LIA_DATA_Y_MSB = 0x2B; 273 | static const uint8_t LIA_DATA_Z_LSB = 0x2C; 274 | static const uint8_t LIA_DATA_Z_MSB = 0x2D; 275 | 276 | static const uint8_t GRV_DATA_X_LSB = 0x2E; 277 | static const uint8_t GRV_DATA_X_MSB = 0x2F; 278 | static const uint8_t GRV_DATA_Y_LSB = 0x30; 279 | static const uint8_t GRV_DATA_Y_MSB = 0x31; 280 | static const uint8_t GRV_DATA_Z_LSB = 0x32; 281 | static const uint8_t GRV_DATA_Z_MSB = 0x33; 282 | 283 | static const uint8_t TEMP = 0x34; 284 | static const uint8_t CALIB_STAT = 0x35; 285 | 286 | static const uint8_t UNIT_SEL = 0x3B; 287 | /// Bitfield offsets for UNIT_SEL 288 | static const uint8_t ACC_UNIT= 0; 289 | static const uint8_t GYR_UNIT = 1; 290 | static const uint8_t EUL_UNIT = 2; 291 | static const uint8_t TEMP_UNIT = 3; 292 | 293 | static const uint8_t OPR_MODE = 0x3D; 294 | /// Bitfield offsets for OPR_MODE 295 | static const uint8_t OPR_MODE_OFFSET = 0; /// bits <3:0> 296 | /// Bitmasks for OPR_MODE 297 | static const uint8_t OPR_MODE_MASK = 0x0F; /// 0b00001111 298 | 299 | static const uint8_t AXIS_MAP_CONFIG = 0x41; 300 | /// Bitfield offsets for AXIS_MAP_CONFIG 301 | static const uint8_t REMAPPED_X_OFFSET = 0; 302 | static const uint8_t REMAPPED_Y_OFFSET = 2; 303 | static const uint8_t REMAPPED_Z_OFFSET = 4; 304 | /// Bitmasks for AXIS_MAP_CONFIG 305 | static const uint8_t AXIS_MASK = 0x03; 306 | 307 | static const uint8_t AXIS_MAP_SIGN = 0x42; 308 | /// Bitfield offsets for AXIS_MAP_SIGN 309 | static const uint8_t REMAPPED_X_SIGN_OFFSET = 2; 310 | static const uint8_t REMAPPED_Y_SIGN_OFFSET = 1; 311 | static const uint8_t REMAPPED_Z_SIGN_OFFSET = 0; 312 | 313 | static const uint8_t ACC_OFFSET_X_LSB = 0x55; 314 | static const uint8_t ACC_OFFSET_X_MSB = 0x56; 315 | static const uint8_t ACC_OFFSET_Y_LSB = 0x57; 316 | static const uint8_t ACC_OFFSET_Y_MSB = 0x58; 317 | static const uint8_t ACC_OFFSET_Z_LSB = 0x59; 318 | static const uint8_t ACC_OFFSET_Z_MSB = 0x5A; 319 | 320 | static const uint8_t MAG_OFFSET_X_LSB = 0x5B; 321 | static const uint8_t MAG_OFFSET_X_MSB = 0x5C; 322 | static const uint8_t MAG_OFFSET_Y_LSB = 0x5D; 323 | static const uint8_t MAG_OFFSET_Y_MSB = 0x5E; 324 | static const uint8_t MAG_OFFSET_Z_LSB = 0x5F; 325 | static const uint8_t MAG_OFFSET_Z_MSB = 0x60; 326 | 327 | static const uint8_t GYR_OFFSET_X_LSB = 0x61; 328 | static const uint8_t GYR_OFFSET_X_MSB = 0x62; 329 | static const uint8_t GYR_OFFSET_Y_LSB = 0x63; 330 | static const uint8_t GYR_OFFSET_Y_MSB = 0x64; 331 | static const uint8_t GYR_OFFSET_Z_LSB = 0x65; 332 | static const uint8_t GYR_OFFSET_Z_MSB = 0x66; 333 | 334 | static const uint8_t ACC_RADIUS_LSB = 0x67; 335 | static const uint8_t ACC_RADIUS_MSB = 0x68; 336 | static const uint8_t MAG_RADIUS_LSB = 0x69; 337 | static const uint8_t MAG_RADIUS_MSB = 0x6A; 338 | 339 | static const uint32_t POWER_ON_RESET_TIME_MS = 650; 340 | }; 341 | #endif //BNO055_H_ 342 | 343 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/README.md: -------------------------------------------------------------------------------- 1 | # i2c_t3 2 | Enhanced I2C library for Teensy 3.0/3.1/LC devices 3 | 4 | This is an enhanced I2C library for [Teensy 3.0/3.1/LC devices](http://pjrc.com/teensy/index.html). 5 | 6 | Recent discussion and a usage summary can be found in the [PJRC forums here](https://forum.pjrc.com/threads/21680-New-I2C-library-for-Teensy3). 7 | 8 | ## **Description** 9 | 10 | To use the library, unpack the library contents into your sketchbook/libraries folder. 11 | 12 | To use with existing Arduino sketches, simply change the **#include \** to **#include \** 13 | 14 | Example sketches can be found in the Arduino menus at: **File->Examples->i2c_t3** 15 | 16 | The latest version of the library provides the following: 17 | 18 | * For **Teensy 3.0**, the I2C interface is: **Wire** 19 | * For **Teensy 3.1 & LC**, the two I2C interfaces are: **Wire** and **Wire1** 20 | 21 | Some interfaces have two sets of pins that they can utilize. Only one set of pins can be used at a time, but in a Master configuration the pins can be changed when the bus is idle. 22 | The **Wire** bus will communicate on pins: 23 | * **18(SDA0)/19(SCL0)** and **16(SCL0)/17(SDA0)** 24 | 25 | The **Wire1** bus will communicate on pins: 26 | * **Teensy 3.1** - **29(SCL1)/30(SDA1)** and **26(SCL1)/31(SDA1)** 27 | * **Teensy LC** - **22(SCL1)/23(SDA1)** 28 | 29 | On Teensy 3.1 the Wire1 connections are all on the surface mount backside pads. It is recommended to use a breakout expansion board to access those, as the pads are likely not mechanically "robust", with respect to soldered wires pulling on them. 30 | 31 | As far as voltage levels, the Teensy 3.0 & LC pins are 3.3V tolerant, and the Teensy 3.1 pins are 5V tolerant. To connect 5V devices to Teensy 3.0/LC or to connect multiple voltage level I2C buses, refer to the following app note by NXP: 32 | http://www.nxp.com/documents/application_note/AN10441.pdf 33 | 34 | The following sections outline the included examples, modifiable header defines, and function summary. Most all functions are demonstrated in the example files. 35 | 36 | ## **Clocking** 37 | 38 | The library now supports all Teensyduino **F_BUS** frequencies: **60MHz, 56MHz, 48MHz, 36MHz, 24MHz, 16MHz, 8MHz, 4MHz, 2MHz**. 39 | 40 | The supported rates depend on the F_BUS setting which in turn depends on the F_CPU setting. The current F_CPU -> F_BUS mapping (Teensyduino 1.21), is as follows. For a given F_BUS, if an unsupported rate is given, then the highest supported rate available is used (since unsupported rates fall off from the high end). An exception is the Wire1 bus on Teensy LC which uses the F_CPU setting directly. 41 | 42 | ``` 43 | I2C_RATE (kHz) 44 | F_CPU F_BUS 3000 2800 2400 2000 1800 1500 1200 1000 800 600 400 300 200 100 45 | ----- ----- --------------------------------------------------------------------- 46 | 168M 56M y y y y y y y y y y y y y 47 | 144M 48M y y y y y y y y y y y y 48 | 120M 60M y y y y y y y y y y y y y y 49 | 96M 48M y y y y y y y y y y y y 50 | 72M 36M y y y y y y y y y y 51 | 48M 48M y y y y y y y y y y y y 52 | 24M 24M y y y y y y y y 53 | 16M 16M y y y y y y 54 | 8M 8M y y y y 55 | 4M 4M y y 56 | 2M 2M y 57 | ``` 58 | 59 | For Teensy 3.0/3.1, under normal bus frequencies (F_BUS=48MHz) the max supported rate is I2C_RATE_2400. The higher rates are only achievable using an overclocked F_CPU setting. 60 | 61 | The Teensy LC has a maximum rate of I2C_RATE_1200 on Wire, and I2C_RATE_2400 on Wire1. 62 | 63 | The rates are not directly equivalent to SCL clock speeds. The peripheral limits the actual SCL speeds to well below the theoretical speeds. To get a better idea of throughput the transfer time for a 128 byte transfer across different F_CPU / F_BUS / I2C_RATE combinations (specifically the interesting overclock speeds) has been measured. This is shown below. 64 | 65 | ![I2C Speed Test](speedtest.jpg) 66 | 67 | ## **Operational Modes** 68 | 69 | There are three modes of operation: **Interrupt**, **DMA**, and **Immediate**. The operating mode of the I2C can be set in the **begin()** or **setOpMode()** functions, using the opMode parameter which can have the following values: 70 | 71 | * I2C_OP_MODE_ISR - Interrupt 72 | * I2C_OP_MODE_DMA - DMA 73 | * I2C_OP_MODE_IMM - Immediate 74 | 75 | **Interrupt** mode is the normal default mode (it was the only mode in library versions prior to v7). It supports both Master and Slave operation. The two other modes, **DMA** and **Immediate**, are for Master operation only. 76 | 77 | DMA mode requires an available DMA channel to operate. In cases where DMA mode is specified, but there are no available channels, then the I2C will revert to operating in Interrupt mode. 78 | 79 | Similarly, for Interrupt mode to work the I2C ISRs must run at a higher priority than the calling function. Where this is not the case, the library will first attempt to elevate the priority of the I2C ISR to a higher priority than the calling function. If that is not possible then it will revert to operating in Immediate mode. 80 | 81 | ## **Example List** 82 | 83 | ### **Teensy 3.0/3.1/LC Examples** 84 | 85 | **master** - this creates a Master device which is setup to talk to the Slave device given in the slave sketch. 86 | 87 | **multiplexed_master** - this creates a Master device which can output I2C commands on either pins 18/19 or pins 16/17, and change pins on-the-fly. It can therefore operate as a Master on two I2C buses (in multiplexed mode, not simultaneous). 88 | 89 | **scanner** - this creates a Master device which will scan the address space and report all devices which ACK. 90 | 91 | **slave** - this creates a Slave device with simple read/write commands and a small addressable memory. 92 | 93 | **slave_range** - this creates a Slave device which will respond to a range of I2C addresses. A function exists to obtain the Rx address, therefore it can be used to make devices which act as multiple I2C Slaves. 94 | 95 | **timeout** - this creates a Master device which is setup to talk to the Slave device given in the slave sketch. It illustrates the use of different timeout functions. 96 | 97 | **interrupt** - this creates a Master device which is setup to periodically read from a Slave device using a timer interrupt. 98 | 99 | ### **Teensy 3.1/LC ONLY Examples:** 100 | 101 | **dual_bus_master_slave** - this creates a device using one bus as a Master and one bus as a Slave. This is particularly useful for Master/Slave development as they can be wired together, creating a closed test environment in a single device. 102 | 103 | ### **Teensy 3.1 ONLY Examples:** 104 | 105 | **quad_master** - utilizing both I2C interfaces and both sets of pins for each interface, this creates a device which can operate as a Master on four independent buses. The Wire bus will communicate on pins 18(SDA0)/19(SCL0) and 16(SCL0)/17(SDA0). The Wire1 bus will communicate on pins 29(SCL1)/30(SDA1) and 26(SCL1)/31(SDA1). 106 | 107 | ## **Header Defines** 108 | 109 | These defines can be modified at the top of the **i2c_t3.h** file. 110 | 111 | * **I2C_BUS_ENABLE n** - this is a Teensy 3.1 only define, which controls how many buses are enabled. When set as "I2C_BUS_ENABLE 1" only Wire will be active and code/ram size will be equivalent to Teensy 3.0. When set as "I2C_BUS_ENABLE 2" then both Wire and Wire1 will be active and code/ram usage will be increased. 112 | 113 | * **I2C_TX_BUFFER_LENGTH n** 114 | * **I2C_RX_BUFFER_LENGTH n** - these two defines control the buffers allocated to transmit/receive functions. When dealing with Slaves which don't need large communication (eg. sensors or such), these buffers can be reduced to a smaller size. Buffers should be large enough to hold: Target Addr + Target Command (varies with protocol) + Data payload. Default is: 259 bytes = 1 byte Addr + 2 byte Command + 256 byte Data. 115 | 116 | * **I2C0_INTR_FLAG_PIN p** 117 | * **I2C1_INTR_FLAG_PIN p** - these defines make the specified pin high whenever the I2C interrupt occurs (I2C0 == Wire, and I2C1 == Wire1). This is useful as a trigger signal when using a logic analyzer. By default they are undefined (commented out). 118 | 119 | * **I2C_AUTO_RETRY** - this define is used to make the library automatically call resetBus() if it has a timeout while trying to send a START. This is useful for clearing a hung Slave device from the bus. If successful it will try again to send the START, and proceed normally. If not then it will exit with a timeout. Note - this option is NOT compatible with multi-master buses. By default it is enabled. 120 | 121 | ## **Function Summary** 122 | 123 | The functions are divided into two classifications: 124 | 125 | * _**Italic**_ functions are compatible with the original Arduino Wire API. This allows existing Arduino sketches to compile without modification. 126 | * **Bold** functions are the added enhanced functions. They utilize the advanced capabilities of the Teensy 3.0/3.1 hardware. The library provides the greatest benefit when utilizing these functions (versus the standard Wire library). 127 | 128 | 129 | _**Wire.begin();**_ - initializes I2C as Master mode, external pullups, 100kHz rate, pins 18/19 (Wire), pins 29/30 (Wire1 on 3.1), pins 22/23 (Wire1 on LC) 130 | 131 | * return: none 132 | 133 | 134 | _**Wire.begin(address);**_ - initializes I2C as Slave mode using address, external pullups, 100kHz rate, pins 18/19 (Wire), pins 29/30 (Wire1 on 3.1), pins 22/23 (Wire1 on LC) 135 | 136 | * return: none 137 | * parameters: 138 | * address = 7bit slave address of device 139 | 140 | 141 | **Wire.begin(mode, address, pins, pullup, rate);** - initializes I2C as Master or single address Slave 142 | 143 | * return: none 144 | * parameters: 145 | * mode = I2C_MASTER, I2C_SLAVE 146 | * address = 7bit slave address when configured as Slave (ignored for Master mode) 147 | * pins = Wire: I2C_PINS_18_19, I2C_PINS_16_17 | Wire1(3.1): I2C_PINS_29_30, I2C_PINS_26_31 | Wire1(LC): I2C_PINS_22_23 148 | * pullup = I2C_PULLUP_EXT, I2C_PULLUP_INT 149 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 150 | 151 | 152 | **Wire.begin(mode, address, pins, pullup, rate, opMode);** - initializes I2C as Master or single address Slave 153 | 154 | * return: none 155 | * parameters: 156 | * mode = I2C_MASTER, I2C_SLAVE 157 | * address = 7bit slave address when configured as Slave (ignored for Master mode) 158 | * pins = Wire: I2C_PINS_18_19, I2C_PINS_16_17 | Wire1(3.1): I2C_PINS_29_30, I2C_PINS_26_31 | Wire1(LC): I2C_PINS_22_23 159 | * pullup = I2C_PULLUP_EXT, I2C_PULLUP_INT 160 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 161 | * opMode = I2C_OP_MODE_ISR, I2C_OP_MODE_DMA, I2C_OP_MODE_IMM 162 | 163 | 164 | **Wire.begin(mode, address1, address2, pins, pullup, rate);** - initializes I2C as Master or address range Slave 165 | 166 | * return: none 167 | * parameters: 168 | * mode = I2C_MASTER, I2C_SLAVE 169 | * address1 = 1st 7bit address for specifying Slave address range (ignored for Master mode) 170 | * address2 = 2nd 7bit address for specifying Slave address range (ignored for Master mode) 171 | * pins = Wire: I2C_PINS_18_19, I2C_PINS_16_17 | Wire1(3.1): I2C_PINS_29_30, I2C_PINS_26_31 | Wire1(LC): I2C_PINS_22_23 172 | * pullup = I2C_PULLUP_EXT, I2C_PULLUP_INT 173 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 174 | 175 | 176 | **Wire.begin(mode, address1, address2, pins, pullup, rate, opMode);** - initializes I2C as Master or address range Slave 177 | 178 | * return: none 179 | * parameters: 180 | * mode = I2C_MASTER, I2C_SLAVE 181 | * address1 = 1st 7bit address for specifying Slave address range (ignored for Master mode) 182 | * address2 = 2nd 7bit address for specifying Slave address range (ignored for Master mode) 183 | * pins = Wire: I2C_PINS_18_19, I2C_PINS_16_17 | Wire1(3.1): I2C_PINS_29_30, I2C_PINS_26_31 | Wire1(LC): I2C_PINS_22_23 184 | * pullup = I2C_PULLUP_EXT, I2C_PULLUP_INT 185 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 186 | * opMode = I2C_OP_MODE_ISR, I2C_OP_MODE_DMA, I2C_OP_MODE_IMM 187 | 188 | 189 | **Wire.setOpMode(opMode);** - this configures operating mode of the I2C as either Immediate, ISR, or DMA. By default Arduino-style begin() calls will initialize to ISR mode. This can only be called when the bus is idle (no changing mode in the middle of Tx/Rx). Note that Slave mode can only use ISR operation. 190 | 191 | * return: 1=success, 0=fail (bus busy) 192 | * parameters: 193 | * opMode = I2C_OP_MODE_ISR, I2C_OP_MODE_DMA, I2C_OP_MODE_IMM 194 | 195 | 196 | **Wire.setRate(busFreq, rate);** - reconfigures I2C frequency divider based on supplied bus freq and desired rate (rate in this case is an enum). If an unsupported busFreq / rate combination is specified, then the highest available rate will be used, and it will flag an error. Bus frequency is supplied here to allow for applications that alter their running frequency to recalibrate the I2C to a known rate given their frequency. Bus frequency must still be one of the supported frequencies. 197 | 198 | * return: 1=success, 0=fail (incompatible bus freq & I2C rate combination) 199 | * parameters: 200 | * busFreq = bus frequency, typically F_BUS unless reconfigured 201 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 202 | 203 | 204 | **Wire.setRate(rate);** - reconfigures I2C frequency divider for desired rate (rate in this case is an enum). It will automatically determine the correct bus frequency for the interface (normally F_BUS, but LC Wire1 uses F_CPU). If an unsupported rate is specified, then the highest available rate will be used, and it will flag an error. 205 | 206 | * return: 1=success, 0=fail (incompatible bus freq & I2C rate combination) 207 | * parameters: 208 | * busFreq = bus frequency, typically F_BUS unless reconfigured 209 | * rate = I2C_RATE_100, I2C_RATE_200, I2C_RATE_300, I2C_RATE_400, I2C_RATE_600, I2C_RATE_800, I2C_RATE_1000, I2C_RATE_1200, I2C_RATE_1500, I2C_RATE_1800, I2C_RATE_2000, I2C_RATE_2400, I2C_RATE_2800, I2C_RATE_3000 210 | 211 | 212 | **Wire.setRate(busFreq, i2cFreq);** - reconfigures I2C frequency divider based on supplied bus freq and desired I2C freq. I2C frequency in this case is quantized to an approximate I2C_RATE based on actual SCL frequency measurements using 48MHz bus as a basis. This is done for simplicity, as theoretical SCL freq do not correlate to actual freq very well. 213 | 214 | * return: 1=success, 0=fail (incompatible bus freq & I2C rate combination) 215 | * parameters: 216 | * busFreq = bus frequency, typically F_BUS unless reconfigured 217 | * i2cFreq = desired I2C frequency (will be quantized to nearest rate) 218 | 219 | 220 | _**Wire.setClock(i2cFreq);**_ - reconfigures I2C frequency divider to get desired I2C freq. Bus frequency is fixed at F_BUS. I2C frequency in this case is quantized to an approximate I2C_RATE based on actual SCL frequency measurements using 48MHz bus as a basis. This is done for simplicity, as theoretical SCL freq do not correlate to actual freq very well. 221 | 222 | * return: 1=success, 0=fail (incompatible bus freq & I2C rate combination) 223 | * parameters: 224 | * i2cFreq = desired I2C frequency (will be quantized to nearest rate) 225 | 226 | 227 | **Wire.pinConfigure(pins, pullup);** - reconfigures active I2C pins on-the-fly (only works when bus is idle). Inactive pins will switch to input mode. 228 | 229 | * return: 1=success, 0=fail 230 | * parameters: 231 | * pins = (Wire) I2C_PINS_18_19, I2C_PINS_16_17 or (Wire1) I2C_PINS_29_30, I2C_PINS_26_31 232 | * pullup = I2C_PULLUP_EXT, I2C_PULLUP_INT 233 | 234 | 235 | **Wire.setDefaultTimeout(timeout);** - sets the default timeout applied to all function calls which do not explicitly set a timeout. The default is initially zero (infinite wait). 236 | 237 | * return: none 238 | * parameters: 239 | * timeout = timeout in microseconds 240 | 241 | 242 | **Wire.resetBus();** - this is used to try and reset the bus in cases of a hung Slave device (typically a Slave which is stuck outputting a low on SDA due to a lost clock). It will generate up to 9 clocks pulses on SCL in an attempt to get the Slave to release the SDA line. Once SDA is released it will restore I2C functionality. 243 | 244 | * return: none 245 | 246 | 247 | _**Wire.beginTransmission(address);**_ - initialize Tx buffer for transmit to slave at address 248 | 249 | * return: none 250 | * parameters: 251 | * address = target 7bit slave address 252 | 253 | 254 | _**Wire.endTransmission();**_ - blocking routine, transmits Tx buffer to slave 255 | 256 | * return: 0=success, 1=data too long, 2=recv addr NACK, 3=recv data NACK, 4=other error 257 | 258 | 259 | **Wire.endTransmission(i2c_stop);** - blocking routine, transmits Tx buffer to slave. i2c_stop parameter can be used to indicate if command should end with a STOP (I2C_STOP) or not (I2C_NOSTOP). 260 | 261 | * return: 0=success, 1=data too long, 2=recv addr NACK, 3=recv data NACK, 4=other error 262 | * parameters: 263 | * i2c_stop = I2C_NOSTOP, I2C_STOP 264 | 265 | 266 | **Wire.endTransmission(i2c_stop, timeout);** - blocking routine with timeout, transmits Tx buffer to slave. i2c_stop parameter can be used to indicate if command should end with a STOP(I2C_STOP) or not (I2C_NOSTOP). 267 | 268 | * return: 0=success, 1=data too long, 2=recv addr NACK, 3=recv data NACK, 4=other error 269 | * parameters: 270 | * i2c_stop = I2C_NOSTOP, I2C_STOP 271 | * timeout = timeout in microseconds 272 | 273 | 274 | **Wire.sendTransmission();** - non-blocking routine, starts transmit of Tx buffer to slave (implicit I2C_STOP). Use done() or finish() to determine completion and status() to determine success/fail. 275 | 276 | * return: none 277 | 278 | 279 | **Wire.sendTransmission(i2c_stop);** - non-blocking routine, starts transmit of Tx buffer to slave. i2c_stop parameter can be used to indicate if command should end with a STOP (I2C_STOP) or not (I2C_NOSTOP). Use done() or finish() to determine completion and status() to determine success/fail. 280 | 281 | * return: none 282 | * parameters: 283 | * i2c_stop = I2C_NOSTOP, I2C_STOP 284 | 285 | 286 | _**Wire.requestFrom(address, length);**_ - blocking routine, requests length bytes from slave at address. Receive data will be placed in the Rx buffer. 287 | 288 | * return: #bytes received = success, 0=fail 289 | * parameters: 290 | * address = target 7bit slave address 291 | * length = number of bytes requested 292 | 293 | 294 | **Wire.requestFrom(address, length, i2c_stop);** - blocking routine, requests length bytes from slave at address. Receive data will be placed in the Rx buffer. i2c_stop parameter can be used to indicate if command should end with a STOP (I2C_STOP) or not (I2C_NOSTOP). 295 | 296 | * return: #bytes received = success, 0=fail 297 | * parameters: 298 | * address = target 7bit slave address 299 | * length = number of bytes requested 300 | * i2c_stop = I2C_NOSTOP, I2C_STOP 301 | 302 | 303 | **Wire.requestFrom(address, length, i2c_stop, timeout);** - blocking routine with timeout, requests length bytes from slave at address. Receive data will be placed in the Rx buffer. i2c_stop parameter can be used to indicate if command should end with a STOP (I2C_STOP) or not (I2C_NOSTOP). 304 | 305 | * return: #bytes received = success, 0=fail 306 | * parameters: 307 | * address = target 7bit slave address 308 | * length = number of bytes requested 309 | * i2c_stop = I2C_NOSTOP, I2C_STOP 310 | * timeout = timeout in microseconds 311 | 312 | 313 | **Wire.sendRequest(address, length, i2c_stop);** - non-blocking routine, starts request for length bytes from slave at address. Receive data will be placed in the Rx buffer. i2c_stop parameter can be used to indicate if command should end with a STOP (I2C_STOP) or not (I2C_NOSTOP). Use done() or finish() to determine completion and status() to determine success/fail. 314 | 315 | * return: none 316 | * parameters: 317 | * address = target 7bit slave address 318 | * length = number of bytes requested 319 | * i2c_stop = I2C_NOSTOP, I2C_STOP 320 | 321 | 322 | **Wire.getError();** - returns "Wire" error code from a failed Tx/Rx command 323 | 324 | * return: 0=success, 1=data too long, 2=recv addr NACK, 3=recv data NACK, 4=other error 325 | 326 | 327 | **Wire.status();** - returns current status of I2C (enum return value) 328 | 329 | * return: I2C_WAITING, I2C_SENDING, I2C_SEND_ADDR, I2C_RECEIVING, I2C_TIMEOUT, I2C_ADDR_NAK, I2C_DATA_NAK, I2C_ARB_LOST, I2C_SLAVE_TX, I2C_SLAVE_RX 330 | 331 | 332 | **Wire.done();** - returns simple complete/not-complete value to indicate I2C status 333 | 334 | * return: 1=Tx/Rx complete (with or without errors), 0=still running 335 | 336 | 337 | **Wire.finish();** - blocking routine, loops until Tx/Rx is complete 338 | 339 | * return: 1=Tx/Rx complete (Tx or Rx completed, no error), 0=fail (NAK, timeout or Arb lost) 340 | 341 | 342 | **Wire.finish(timeout);** - blocking routine with timeout, loops until Tx/Rx is complete or timeout occurs 343 | 344 | * return: 1=Tx/Rx complete (Tx or Rx completed, no error), 0=fail (NAK, timeout or Arb lost) 345 | * parameters: 346 | * timeout = timeout in microseconds 347 | 348 | 349 | _**Wire.write(data);**_ - write data byte to Tx buffer 350 | 351 | * return: #bytes written = success, 0=fail 352 | * parameters: 353 | * data = data byte 354 | 355 | 356 | _**Wire.write(data, length);**_ - write length number of bytes from data array to Tx buffer 357 | 358 | * return: #bytes written = success, 0=fail 359 | * parameters: 360 | * data = pointer to uint8_t (or char) array of data 361 | * length = number of bytes to write 362 | 363 | 364 | _**Wire.available();**_ - returns number of remaining available bytes in Rx buffer 365 | 366 | * return: #bytes available 367 | 368 | 369 | _**Wire.read();**_ - returns next data byte (signed int) from Rx buffer 370 | 371 | * return: data, -1 if buffer empty 372 | 373 | 374 | _**Wire.peek();**_ - returns next data byte (signed int) from Rx buffer without removing it from Rx buffer 375 | 376 | * return: data, -1 if buffer empty 377 | 378 | 379 | **Wire.readByte();** - returns next data byte (uint8_t) from Rx buffer 380 | 381 | * return: data, 0 if buffer empty 382 | 383 | 384 | **Wire.peekByte();** - returns next data byte (uint8_t) from Rx buffer without removing it from Rx buffer 385 | 386 | * return: data, 0 if buffer empty 387 | 388 | _**Wire.flush();**_ - does nothing 389 | 390 | 391 | **Wire.getRxAddr();** - returns target address of incoming I2C command. Used for Slaves operating over an address range. 392 | 393 | * return: rxAddr of last received command 394 | 395 | 396 | _**Wire.onReceive(function);**_ - used to set Slave Rx callback, refer to code examples 397 | 398 | 399 | _**Wire.onRequest(function);**_ - used to set Slave Tx callback, refer to code examples 400 | 401 | 402 | ## **Compatible Libraries** 403 | 404 | These are libraries which are known to be compatible with this I2C library. They may have been possibly modified to utilize enhanced functions (higher speed, timeouts, etc), or perhaps for general compatibility. Please contact their respective authors for questions regarding their usage. 405 | 406 | * Arduino sketch for MPU-9250 9DoF with AHRS sensor fusion - https://github.com/kriswiner/MPU-9250 407 | * LSM9DS0 9DOF sensor AHRS sketch - https://github.com/kriswiner/LSM9DS0 408 | * Adafruit FRAM board - https://bitbucket.org/JezWeston/adafruit_fram_i2c_t3 -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/dual_bus_master_slave/dual_bus_master_slave.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.1/LC I2C Dual Bus Master Slave 3 | // 18Jan14 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // ********************************************************************************************** 7 | // ** Note: This is a Teensy 3.1/LC ONLY sketch since it requires a device with two I2C buses. ** 8 | // ********************************************************************************************** 9 | // 10 | // This creates a device using one I2C bus as a Master and one I2C bus as a Slave. 11 | // 12 | // The "Wire" bus will be setup as a Master on pins 18(SDA0)/19(SCL0). 13 | // The "Wire1" bus will be setup as a Slave on pins 29(SCL1)/30(SDA1) (T3.1) or 22(SCL1)/23(SDA1) (LC) 14 | // 15 | // The device can be setup to talk to other devices, or it can connect the buses together 16 | // and talk to itself. This allows Master/Slave development on a single device. The example 17 | // is setup for external pullups, so it will require external resistors on SCL & SDA. 18 | // 19 | // Pulling pin12 low will initiate a WRITE then READ transfer between Master and Slave. 20 | // 21 | // This example code is in the public domain. 22 | // 23 | // ------------------------------------------------------------------------------------------- 24 | // Slave protocol is as follows: 25 | // ------------------------------------------------------------------------------------------- 26 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 27 | // a memory address to store to, and a sequence of data to store. 28 | // The command sequence is: 29 | // 30 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 31 | // 32 | // where START = I2C START sequence 33 | // I2CADDR+W = I2C Slave address + I2C write flag 34 | // WRITE = WRITE command 35 | // MEMADDR = memory address to store data to 36 | // DATAx = data byte to store, multiple bytes are stored to increasing address 37 | // STOP = I2C STOP sequence 38 | // ------------------------------------------------------------------------------------------- 39 | // READ - The I2C Master can read data from the device by transmitting the READ command, 40 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 41 | // followed by reading the data. The command sequence is: 42 | // 43 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 44 | // 45 | // where START = I2C START sequence 46 | // I2CADDR+W = I2C Slave address + I2C write flag 47 | // READ = READ command 48 | // MEMADDR = memory address to read data from 49 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 50 | // I2CADDR+R = I2C Slave address + I2C read flag 51 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 52 | // STOP = I2C STOP sequence 53 | // ------------------------------------------------------------------------------------------- 54 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 55 | // The command sequence is: 56 | // 57 | // START|I2CADDR+W|SETRATE|RATE|STOP 58 | // 59 | // where START = I2C START sequence 60 | // I2CADDR+W = I2C Slave address + I2C write flag 61 | // SETRATE = SETRATE command 62 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 63 | // ------------------------------------------------------------------------------------------- 64 | 65 | #include 66 | 67 | // Command definitions 68 | #define WRITE 0x10 69 | #define READ 0x20 70 | #define SETRATE 0x30 71 | 72 | // Function prototypes 73 | void receiveEvent(size_t len); 74 | void requestEvent(void); 75 | void printWireStatus(void); 76 | void printWire1Status(void); 77 | void printStatus(i2c_status status); 78 | 79 | // Slave Memory 80 | #define MEM_LEN 256 81 | uint8_t mem[MEM_LEN]; 82 | uint8_t cmd; 83 | size_t addr; 84 | i2c_rate rate; 85 | 86 | void setup() 87 | { 88 | pinMode(LED_BUILTIN,OUTPUT); // LED 89 | pinMode(12,INPUT_PULLUP); // Control for Test1 90 | 91 | // Wire - Setup for Master mode, pins 18/19, external pullup, 400kHz 92 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 93 | 94 | // Wire1 - Setup for Slave mode, address 0x44, pins 29/30 (T3.1) or 22/23 (LC), 95 | // external pullup, 400kHz 96 | #if defined(__MK20DX256__) 97 | Wire1.begin(I2C_SLAVE, 0x44, I2C_PINS_29_30, I2C_PULLUP_EXT, I2C_RATE_400); 98 | #else 99 | Wire1.begin(I2C_SLAVE, 0x44, I2C_PINS_22_23, I2C_PULLUP_EXT, I2C_RATE_400); 100 | #endif 101 | // Slave init 102 | cmd = 0; 103 | addr = 0; 104 | memset(mem, 0, sizeof(mem)); 105 | 106 | // register Slave events 107 | Wire1.onReceive(receiveEvent); 108 | Wire1.onRequest(requestEvent); 109 | 110 | Serial.begin(115200); 111 | } 112 | 113 | // 114 | // Master Loop 115 | // 116 | void loop() 117 | { 118 | uint8_t target = 0x44; // target address 119 | uint8_t databuf[256]; 120 | uint8_t addr = 0, len; 121 | 122 | // 123 | // Test1 - pull pin12 low to send command to slave 124 | // 125 | 126 | if(digitalRead(12) == LOW) 127 | { 128 | uint8_t fail, data; 129 | 130 | digitalWrite(LED_BUILTIN,HIGH); // LED on 131 | Serial.print("------------------------------------------------------------\n"); 132 | Serial.print("Test1 : This will WRITE then READ a 32 byte block from\n"); 133 | Serial.print(" the Master device (Wire) to the Slave device (Wire1)\n"); 134 | Serial.print("------------------------------------------------------------\n"); 135 | 136 | for(len = 0; len < 32; len++) // prepare data to send 137 | databuf[len] = random(0xFF); // set random data 138 | 139 | Serial.printf("I2C WRITE 32 bytes to Slave 0x%0X at MemAddr %d\n", target, addr); 140 | Serial.print("Writing: "); 141 | for(len = 0; len < 32; len++) { Serial.printf("%d ",databuf[len]); } 142 | Serial.print("\n"); 143 | 144 | Wire.beginTransmission(target); // slave addr 145 | Wire.write(WRITE); // WRITE command 146 | Wire.write(addr); // memory address 147 | for(len = 0; len < 32; len++) // write 32 byte block 148 | Wire.write(databuf[len]); 149 | Wire.endTransmission(); // blocking write (when not specified I2C_STOP is implicit) 150 | fail = Wire.getError(); 151 | 152 | printWireStatus(); // print I2C final status 153 | 154 | // Reading from Slave ------------------------------------------------------ 155 | if(!fail) 156 | { 157 | Wire.beginTransmission(target); // slave addr 158 | Wire.write(READ); // READ command 159 | Wire.write(addr); // memory address 160 | Wire.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 161 | Wire.requestFrom(target,32,I2C_STOP); // blocking read (request 32 bytes) 162 | fail = Wire.getError(); 163 | 164 | Serial.printf("I2C READ %d bytes from Slave 0x%0X at MemAddr %d\n", Wire.available(), target, addr); 165 | Serial.print("Received: "); // print received bytes 166 | for(len = 0; len < 32 && Wire.available(); len++) // verify 32 byte block 167 | { 168 | data = Wire.readByte(); 169 | Serial.printf("%d ", data); 170 | if(databuf[len] != data) fail=1; 171 | } 172 | if(!fail) 173 | Serial.print("\nCORRECT!\n"); 174 | else 175 | Serial.print("\nWRONG!\n"); 176 | } 177 | printWireStatus(); // print I2C final status 178 | 179 | delay(500); // delay to space out tests 180 | digitalWrite(LED_BUILTIN,LOW); // LED off 181 | } 182 | } 183 | 184 | // 185 | // handle Slave Rx Event (incoming I2C request/data) 186 | // 187 | void receiveEvent(size_t len) 188 | { 189 | if(Wire1.available()) 190 | { 191 | // grab command 192 | cmd = Wire1.readByte(); 193 | switch(cmd) 194 | { 195 | case WRITE: 196 | addr = Wire1.readByte(); // grab addr 197 | while(Wire1.available()) 198 | if(addr < MEM_LEN) // drop data beyond mem boundary 199 | mem[addr++] = Wire1.readByte(); // copy data to mem 200 | else 201 | Wire1.readByte(); // drop data if mem full 202 | break; 203 | 204 | case READ: 205 | addr = Wire1.readByte(); // grab addr 206 | break; 207 | 208 | case SETRATE: 209 | rate = (i2c_rate)Wire1.readByte(); // grab rate 210 | Wire1.setRate(rate); // set rate 211 | break; 212 | } 213 | } 214 | } 215 | 216 | // 217 | // handle Slave Tx Event (outgoing I2C data) 218 | // 219 | void requestEvent(void) 220 | { 221 | switch(cmd) 222 | { 223 | case READ: 224 | Wire1.write(&mem[addr], MEM_LEN-addr); // fill Tx buffer (from addr location to end of mem) 225 | break; 226 | } 227 | } 228 | 229 | // 230 | // print I2C status 231 | // 232 | void printWireStatus(void) { printStatus(Wire.status()); } 233 | void printWire1Status(void) { printStatus(Wire1.status()); } 234 | void printStatus(i2c_status status) 235 | { 236 | switch(status) 237 | { 238 | case I2C_WAITING: Serial.print("I2C waiting, no errors\n"); break; 239 | case I2C_ADDR_NAK: Serial.print("Slave addr not acknowledged\n"); break; 240 | case I2C_DATA_NAK: Serial.print("Slave data not acknowledged\n"); break; 241 | case I2C_ARB_LOST: Serial.print("Bus Error: Arbitration Lost\n"); break; 242 | case I2C_TIMEOUT: Serial.print("I2C timeout\n"); break; 243 | case I2C_BUF_OVF: Serial.print("I2C buffer overflow\n"); break; 244 | default: Serial.print("I2C busy\n"); break; 245 | } 246 | } 247 | 248 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/interrupt/interrupt.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Interrupt Test 3 | // 15May13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C master device which will issue I2C commands from inside a periodic 7 | // interrupt (eg. reading a sensor at regular time intervals). For this test the Slave device 8 | // will be assumed to be that given in the i2c_t3/slave sketch. 9 | // 10 | // The test will start when the Serial monitor opens. 11 | // 12 | // This example code is in the public domain. 13 | // 14 | // ------------------------------------------------------------------------------------------- 15 | // Slave protocol is as follows: 16 | // ------------------------------------------------------------------------------------------- 17 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 18 | // a memory address to store to, and a sequence of data to store. 19 | // The command sequence is: 20 | // 21 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 22 | // 23 | // where START = I2C START sequence 24 | // I2CADDR+W = I2C Slave address + I2C write flag 25 | // WRITE = WRITE command 26 | // MEMADDR = memory address to store data to 27 | // DATAx = data byte to store, multiple bytes are stored to increasing address 28 | // STOP = I2C STOP sequence 29 | // ------------------------------------------------------------------------------------------- 30 | // READ - The I2C Master can read data from the device by transmitting the READ command, 31 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 32 | // followed by reading the data. The command sequence is: 33 | // 34 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 35 | // 36 | // where START = I2C START sequence 37 | // I2CADDR+W = I2C Slave address + I2C write flag 38 | // READ = READ command 39 | // MEMADDR = memory address to read data from 40 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 41 | // I2CADDR+R = I2C Slave address + I2C read flag 42 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 43 | // STOP = I2C STOP sequence 44 | // ------------------------------------------------------------------------------------------- 45 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 46 | // The command sequence is: 47 | // 48 | // START|I2CADDR+W|SETRATE|RATE|STOP 49 | // 50 | // where START = I2C START sequence 51 | // I2CADDR+W = I2C Slave address + I2C write flag 52 | // SETRATE = SETRATE command 53 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 54 | // ------------------------------------------------------------------------------------------- 55 | 56 | #include 57 | 58 | // Command definitions 59 | #define WRITE 0x10 60 | #define READ 0x20 61 | #define SETRATE 0x30 62 | 63 | // Function prototypes 64 | void readSlave(void); 65 | 66 | IntervalTimer slaveTimer; 67 | uint8_t target=0x44; 68 | size_t addr=0, len; 69 | 70 | void setup() 71 | { 72 | pinMode(LED_BUILTIN,OUTPUT); // LED 73 | 74 | // Setup for Master mode, pins 18/19, external pullups, 400kHz 75 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 76 | 77 | Serial.begin(115200); 78 | delay(5); // Slave powerup wait 79 | 80 | // Setup Slave 81 | Wire.beginTransmission(target); // slave addr 82 | Wire.write(WRITE); // WRITE command 83 | Wire.write(addr); // memory address 84 | for(len = 0; len < 8; len++) // write 8 byte block 85 | Wire.write(len); 86 | Wire.endTransmission(); // blocking Tx 87 | 88 | while(!Serial); // wait to start 89 | 90 | // Start reading Slave device 91 | slaveTimer.begin(readSlave,1000000); // 1s timer 92 | } 93 | 94 | void loop() 95 | { 96 | delay(1); 97 | } 98 | 99 | void readSlave(void) 100 | { 101 | digitalWrite(LED_BUILTIN,HIGH); // pulse LED when reading 102 | 103 | Wire.beginTransmission(target); // slave addr 104 | Wire.write(READ); // WRITE command 105 | Wire.write(addr); // memory address 106 | Wire.endTransmission(I2C_NOSTOP); // blocking Tx, no STOP 107 | Wire.requestFrom(target,1,I2C_STOP); // READ 1 byte 108 | 109 | Serial.printf("Data read from Slave device: %d\n", Wire.readByte()); 110 | addr = (addr < 7) ? addr+1 : 0; // loop reading memory address 111 | digitalWrite(LED_BUILTIN,LOW); 112 | } 113 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/multiplexed_master/multiplexed_master.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Multiplexed Master 3 | // 08Mar13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C multiplexed master device which talks to the simple I2C slave device 7 | // given in the i2c_t3/slave sketch. It is different than the Master sketch because it can 8 | // output I2C commands on either pins 18/19 or pins 16/17, and change pins on-the-fly. 9 | // 10 | // Note that it is using the same I2C bus on both sets of pins (I2C0). The purpose of this 11 | // sketch is to demonstrate how the pins can be reconfigured on-the-fly when the bus is idle. 12 | // 13 | // This code assumes the slave config has 256byte memory and I2C address is 0x44. 14 | // The various tests are started by pulling one of the control pins low. 15 | // 16 | // This example code is in the public domain. 17 | // 18 | // ------------------------------------------------------------------------------------------- 19 | // Slave protocol is as follows: 20 | // ------------------------------------------------------------------------------------------- 21 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 22 | // a memory address to store to, and a sequence of data to store. 23 | // The command sequence is: 24 | // 25 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 26 | // 27 | // where START = I2C START sequence 28 | // I2CADDR+W = I2C Slave address + I2C write flag 29 | // WRITE = WRITE command 30 | // MEMADDR = memory address to store data to 31 | // DATAx = data byte to store, multiple bytes are stored to increasing address 32 | // STOP = I2C STOP sequence 33 | // ------------------------------------------------------------------------------------------- 34 | // READ - The I2C Master can read data from the device by transmitting the READ command, 35 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 36 | // followed by reading the data. The command sequence is: 37 | // 38 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 39 | // 40 | // where START = I2C START sequence 41 | // I2CADDR+W = I2C Slave address + I2C write flag 42 | // READ = READ command 43 | // MEMADDR = memory address to read data from 44 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 45 | // I2CADDR+R = I2C Slave address + I2C read flag 46 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 47 | // STOP = I2C STOP sequence 48 | // ------------------------------------------------------------------------------------------- 49 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 50 | // The command sequence is: 51 | // 52 | // START|I2CADDR+W|SETRATE|RATE|STOP 53 | // 54 | // where START = I2C START sequence 55 | // I2CADDR+W = I2C Slave address + I2C write flag 56 | // SETRATE = SETRATE command 57 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 58 | // ------------------------------------------------------------------------------------------- 59 | 60 | #include 61 | 62 | // Command definitions 63 | #define WRITE 0x10 64 | #define READ 0x20 65 | #define SETRATE 0x30 66 | 67 | // Function prototypes 68 | void print_i2c_status(void); 69 | 70 | void setup() 71 | { 72 | pinMode(LED_BUILTIN,OUTPUT); // LED 73 | pinMode(12,INPUT_PULLUP); // Control for Test1 74 | pinMode(11,INPUT_PULLUP); // Control for Test2 75 | 76 | // Setup for Master mode, pins 18/19, external pullups, 400kHz 77 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 78 | 79 | Serial.begin(115200); 80 | } 81 | 82 | void loop() 83 | { 84 | uint8_t data = 0x0A, addr = 0; 85 | uint8_t target1 = 0x44; // slave1 addr 86 | uint8_t target2 = 0x44; // slave2 addr 87 | 88 | // 89 | // Multi-bus test 90 | // 91 | // pull pin12 low to send command to slave1 on bus1 (pins 16/17) 92 | // pull pin11 low to send command to slave2 on bus2 (pins 18/19) 93 | // 94 | 95 | if(digitalRead(12) == LOW) 96 | { 97 | // do a 1 byte write on bus1 98 | // 99 | Wire.pinConfigure(I2C_PINS_16_17, I2C_PULLUP_EXT); // change pins 100 | 101 | digitalWrite(LED_BUILTIN,HIGH); // LED on 102 | Serial.print("---------------------------------------------------------\n"); 103 | Serial.print("Test1 : This will WRITE then READ 1 byte to the Slave\n"); 104 | Serial.print(" connected to pins 16/17.\n"); 105 | Serial.print("---------------------------------------------------------\n"); 106 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target1, addr); 107 | 108 | Wire.beginTransmission(target1); // slave addr 109 | Wire.write(WRITE); // WRITE command 110 | Wire.write(addr); // memory address 111 | Wire.write(data); // set data 112 | Wire.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 113 | 114 | print_i2c_status(); // print I2C final status 115 | 116 | // Reading from Slave ------------------------------------------------------ 117 | Wire.beginTransmission(target1); // slave addr 118 | Wire.write(READ); // READ command 119 | Wire.write(addr); // memory address 120 | Wire.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 121 | Wire.requestFrom(target1,1,I2C_STOP); // blocking read (request 1 byte) 122 | 123 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire.readByte(), target1, addr); 124 | print_i2c_status(); // print I2C final status 125 | digitalWrite(LED_BUILTIN,LOW); // LED off 126 | delay(500); // delay to space out tests 127 | } 128 | 129 | if(digitalRead(11) == LOW) 130 | { 131 | // do a 1 byte write on bus2 132 | // 133 | Wire.pinConfigure(I2C_PINS_18_19, I2C_PULLUP_EXT); // change pins 134 | 135 | digitalWrite(LED_BUILTIN,HIGH); // LED on 136 | Serial.print("---------------------------------------------------------\n"); 137 | Serial.print("Test2 : This will WRITE then READ 1 byte to the Slave\n"); 138 | Serial.print(" connected to pins 18/19.\n"); 139 | Serial.print("---------------------------------------------------------\n"); 140 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target2, addr); 141 | 142 | Wire.beginTransmission(target2); // slave addr 143 | Wire.write(WRITE); // WRITE command 144 | Wire.write(addr); // memory address 145 | Wire.write(data); // set data 146 | Wire.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 147 | 148 | print_i2c_status(); // print I2C final status 149 | 150 | // Reading from Slave ------------------------------------------------------ 151 | Wire.beginTransmission(target2); // slave addr 152 | Wire.write(READ); // READ command 153 | Wire.write(addr); // memory address 154 | Wire.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 155 | Wire.requestFrom(target2,1,I2C_STOP); // blocking read (request 1 byte) 156 | 157 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire.readByte(), target2, addr); 158 | print_i2c_status(); // print I2C final status 159 | digitalWrite(LED_BUILTIN,LOW); // LED off 160 | delay(500); // delay to space out tests 161 | } 162 | } 163 | 164 | // 165 | // print I2C status 166 | // 167 | void print_i2c_status(void) 168 | { 169 | switch(Wire.status()) 170 | { 171 | case I2C_WAITING: Serial.print("I2C waiting, no errors\n"); break; 172 | case I2C_ADDR_NAK: Serial.print("Slave addr not acknowledged\n"); break; 173 | case I2C_DATA_NAK: Serial.print("Slave data not acknowledged\n"); break; 174 | case I2C_ARB_LOST: Serial.print("Bus Error: Arbitration Lost\n"); break; 175 | case I2C_TIMEOUT: Serial.print("I2C timeout\n"); break; 176 | case I2C_BUF_OVF: Serial.print("I2C buffer overflow\n"); break; 177 | default: Serial.print("I2C busy\n"); break; 178 | } 179 | } 180 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/quad_master/quad_master.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.1 I2C Quad-Master 3 | // 18Jan14 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // ******************************************************************************************* 7 | // ** Note: This is a Teensy 3.1 ONLY sketch since it requires a device with two I2C buses, ** 8 | // ** each having two sets of multiplexed pins. ** 9 | // ******************************************************************************************* 10 | // 11 | // It creates an I2C quad-master device which utilizes both I2C buses, and both sets of pins 12 | // for each bus, thereby creating a quad-master device. 13 | // 14 | // The "Wire" bus will communicate on pins 18(SDA0)/19(SCL0) and 16(SCL0)/17(SDA0). 15 | // The "Wire1" bus will communicate on pins 29(SCL1)/30(SDA1) and 26(SCL1)/31(SDA1). 16 | // 17 | // This code assumes that each bus has a target device configured with the simple I2C slave 18 | // given in the i2c_t3/slave sketch, with each having a 256byte memory and I2C addr 0x44. 19 | // 20 | // The various tests are started by pulling one of the control pins low as follows: 21 | // 22 | // pull pin9 low to send command to slave on bus1 (pins 18/19) 23 | // pull pin10 low to send command to slave on bus2 (pins 16/17) 24 | // pull pin11 low to send command to slave on bus3 (pins 29/30) 25 | // pull pin12 low to send command to slave on bus4 (pins 26/31) 26 | // 27 | // This example code is in the public domain. 28 | // 29 | // ------------------------------------------------------------------------------------------- 30 | // Slave protocol is as follows: 31 | // ------------------------------------------------------------------------------------------- 32 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 33 | // a memory address to store to, and a sequence of data to store. 34 | // The command sequence is: 35 | // 36 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 37 | // 38 | // where START = I2C START sequence 39 | // I2CADDR+W = I2C Slave address + I2C write flag 40 | // WRITE = WRITE command 41 | // MEMADDR = memory address to store data to 42 | // DATAx = data byte to store, multiple bytes are stored to increasing address 43 | // STOP = I2C STOP sequence 44 | // ------------------------------------------------------------------------------------------- 45 | // READ - The I2C Master can read data from the device by transmitting the READ command, 46 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 47 | // followed by reading the data. The command sequence is: 48 | // 49 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 50 | // 51 | // where START = I2C START sequence 52 | // I2CADDR+W = I2C Slave address + I2C write flag 53 | // READ = READ command 54 | // MEMADDR = memory address to read data from 55 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 56 | // I2CADDR+R = I2C Slave address + I2C read flag 57 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 58 | // STOP = I2C STOP sequence 59 | // ------------------------------------------------------------------------------------------- 60 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 61 | // The command sequence is: 62 | // 63 | // START|I2CADDR+W|SETRATE|RATE|STOP 64 | // 65 | // where START = I2C START sequence 66 | // I2CADDR+W = I2C Slave address + I2C write flag 67 | // SETRATE = SETRATE command 68 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 69 | // ------------------------------------------------------------------------------------------- 70 | 71 | #include 72 | 73 | // Command definitions 74 | #define WRITE 0x10 75 | #define READ 0x20 76 | #define SETRATE 0x30 77 | 78 | // Function prototypes 79 | void printWireStatus(void); 80 | void printWire1Status(void); 81 | void printStatus(i2c_status status); 82 | 83 | void setup() 84 | { 85 | pinMode(LED_BUILTIN,OUTPUT); // LED 86 | pinMode(9,INPUT_PULLUP); // Control for Bus1 87 | pinMode(10,INPUT_PULLUP); // Control for Bus2 88 | pinMode(11,INPUT_PULLUP); // Control for Bus3 89 | pinMode(12,INPUT_PULLUP); // Control for Bus4 90 | 91 | // Setup for Master mode, default pins, 400kHz, and internal pullups 92 | // (note: internal pullups only used here to reduce test board clutter with 4 buses, external generally better) 93 | // 94 | // Wire 95 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_400); 96 | pinMode(16,INPUT_PULLUP); // Alternate Wire pins 16/17 should be initialized as inputs with same pullup style 97 | pinMode(17,INPUT_PULLUP); 98 | // Wire1 99 | Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_29_30, I2C_PULLUP_INT, I2C_RATE_400); 100 | pinMode(26,INPUT_PULLUP); // Alternate Wire1 pins 26/31 should be initialized as inputs with same pullup style 101 | pinMode(31,INPUT_PULLUP); 102 | 103 | Serial.begin(115200); 104 | } 105 | 106 | void loop() 107 | { 108 | // slave addr - note: using identical slave addresses is intentional, as this 109 | // is a common situation with many off-the-shelf devices 110 | uint8_t target = 0x44; 111 | uint8_t data = 0x0A, addr = 0; 112 | 113 | // 114 | // Quad-bus test 115 | // 116 | // pull pin9 low to send command to slave on bus1 (pins 18/19) 117 | // pull pin10 low to send command to slave on bus2 (pins 16/17) 118 | // pull pin11 low to send command to slave on bus3 (pins 29/30) 119 | // pull pin12 low to send command to slave on bus4 (pins 26/31) 120 | // 121 | 122 | if(digitalRead(12) == LOW) 123 | { 124 | // do a 1 byte write on bus1 125 | // 126 | Wire.pinConfigure(I2C_PINS_18_19, I2C_PULLUP_INT); // change pins 127 | 128 | digitalWrite(LED_BUILTIN,HIGH); // LED on 129 | Serial.print("---------------------------------------------------------\n"); 130 | Serial.print("Test1 : This will WRITE then READ 1 byte to the Slave\n"); 131 | Serial.print(" connected to pins 18/19.\n"); 132 | Serial.print("---------------------------------------------------------\n"); 133 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target, addr); 134 | 135 | Wire.beginTransmission(target); // slave addr 136 | Wire.write(WRITE); // WRITE command 137 | Wire.write(addr); // memory address 138 | Wire.write(data); // set data 139 | Wire.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 140 | 141 | printWireStatus(); // print I2C final status 142 | 143 | // Reading from Slave ------------------------------------------------------ 144 | Wire.beginTransmission(target); // slave addr 145 | Wire.write(READ); // READ command 146 | Wire.write(addr); // memory address 147 | Wire.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 148 | Wire.requestFrom(target,1,I2C_STOP); // blocking read (request 1 byte) 149 | 150 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire.readByte(), target, addr); 151 | printWireStatus(); // print I2C final status 152 | digitalWrite(LED_BUILTIN,LOW); // LED off 153 | delay(500); // delay to space out tests 154 | } 155 | 156 | if(digitalRead(11) == LOW) 157 | { 158 | // do a 1 byte write on bus2 159 | // 160 | Wire.pinConfigure(I2C_PINS_16_17, I2C_PULLUP_INT); // change pins 161 | 162 | digitalWrite(LED_BUILTIN,HIGH); // LED on 163 | Serial.print("---------------------------------------------------------\n"); 164 | Serial.print("Test2 : This will WRITE then READ 1 byte to the Slave\n"); 165 | Serial.print(" connected to pins 16/17.\n"); 166 | Serial.print("---------------------------------------------------------\n"); 167 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target, addr); 168 | 169 | Wire.beginTransmission(target); // slave addr 170 | Wire.write(WRITE); // WRITE command 171 | Wire.write(addr); // memory address 172 | Wire.write(data); // set data 173 | Wire.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 174 | 175 | printWireStatus(); // print I2C final status 176 | 177 | // Reading from Slave ------------------------------------------------------ 178 | Wire.beginTransmission(target); // slave addr 179 | Wire.write(READ); // READ command 180 | Wire.write(addr); // memory address 181 | Wire.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 182 | Wire.requestFrom(target,1,I2C_STOP); // blocking read (request 1 byte) 183 | 184 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire.readByte(), target, addr); 185 | printWireStatus(); // print I2C final status 186 | digitalWrite(LED_BUILTIN,LOW); // LED off 187 | delay(500); // delay to space out tests 188 | } 189 | 190 | if(digitalRead(10) == LOW) 191 | { 192 | // do a 1 byte write on bus3 193 | // 194 | Wire1.pinConfigure(I2C_PINS_29_30, I2C_PULLUP_INT); // change pins 195 | 196 | digitalWrite(LED_BUILTIN,HIGH); // LED on 197 | Serial.print("---------------------------------------------------------\n"); 198 | Serial.print("Test3 : This will WRITE then READ 1 byte to the Slave\n"); 199 | Serial.print(" connected to pins 29/30.\n"); 200 | Serial.print("---------------------------------------------------------\n"); 201 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target, addr); 202 | 203 | Wire1.beginTransmission(target); // slave addr 204 | Wire1.write(WRITE); // WRITE command 205 | Wire1.write(addr); // memory address 206 | Wire1.write(data); // set data 207 | Wire1.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 208 | 209 | printWire1Status(); // print I2C final status 210 | 211 | // Reading from Slave ------------------------------------------------------ 212 | Wire1.beginTransmission(target); // slave addr 213 | Wire1.write(READ); // READ command 214 | Wire1.write(addr); // memory address 215 | Wire1.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 216 | Wire1.requestFrom(target,1,I2C_STOP); // blocking read (request 1 byte) 217 | 218 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire1.readByte(), target, addr); 219 | printWire1Status(); // print I2C final status 220 | digitalWrite(LED_BUILTIN,LOW); // LED off 221 | delay(500); // delay to space out tests 222 | } 223 | 224 | if(digitalRead(9) == LOW) 225 | { 226 | // do a 1 byte write on bus4 227 | // 228 | Wire1.pinConfigure(I2C_PINS_26_31, I2C_PULLUP_INT); // change pins 229 | 230 | digitalWrite(LED_BUILTIN,HIGH); // LED on 231 | Serial.print("---------------------------------------------------------\n"); 232 | Serial.print("Test4 : This will WRITE then READ 1 byte to the Slave\n"); 233 | Serial.print(" connected to pins 26/31.\n"); 234 | Serial.print("---------------------------------------------------------\n"); 235 | Serial.printf("Writing 1 byte (0x%0X) to Slave 0x%0X at MemAddr %d\n", data, target, addr); 236 | 237 | Wire1.beginTransmission(target); // slave addr 238 | Wire1.write(WRITE); // WRITE command 239 | Wire1.write(addr); // memory address 240 | Wire1.write(data); // set data 241 | Wire1.endTransmission(); // blocking I2C Tx (when not specified I2C_STOP is implicit) 242 | 243 | printWire1Status(); // print I2C final status 244 | 245 | // Reading from Slave ------------------------------------------------------ 246 | Wire1.beginTransmission(target); // slave addr 247 | Wire1.write(READ); // READ command 248 | Wire1.write(addr); // memory address 249 | Wire1.endTransmission(I2C_NOSTOP); // blocking write (NOSTOP triggers RepSTART on next I2C command) 250 | Wire1.requestFrom(target,1,I2C_STOP); // blocking read (request 1 byte) 251 | 252 | Serial.printf("Read 1 byte (0x%0X) from Slave 0x%0X at MemAddr %d\n", Wire1.readByte(), target, addr); 253 | printWire1Status(); // print I2C final status 254 | digitalWrite(LED_BUILTIN,LOW); // LED off 255 | delay(500); // delay to space out tests 256 | } 257 | } 258 | 259 | // 260 | // print I2C status 261 | // 262 | void printWireStatus(void) { printStatus(Wire.status()); } 263 | void printWire1Status(void) { printStatus(Wire1.status()); } 264 | void printStatus(i2c_status status) 265 | { 266 | switch(status) 267 | { 268 | case I2C_WAITING: Serial.print("I2C waiting, no errors\n"); break; 269 | case I2C_ADDR_NAK: Serial.print("Slave addr not acknowledged\n"); break; 270 | case I2C_DATA_NAK: Serial.print("Slave data not acknowledged\n"); break; 271 | case I2C_ARB_LOST: Serial.print("Bus Error: Arbitration Lost\n"); break; 272 | case I2C_TIMEOUT: Serial.print("I2C timeout\n"); break; 273 | case I2C_BUF_OVF: Serial.print("I2C buffer overflow\n"); break; 274 | default: Serial.print("I2C busy\n"); break; 275 | } 276 | } 277 | 278 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/scanner/scanner.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Scanner 3 | // 08Mar13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C master device which will scan the address space and report all 7 | // devices which ACK. It does not attempt to transfer data, it only reports which devices 8 | // ACK their address. 9 | // 10 | // Pull the control pin low to initiate the scan. Result will output to Serial. 11 | // 12 | // This example code is in the public domain. 13 | // ------------------------------------------------------------------------------------------- 14 | 15 | #include 16 | 17 | // Function prototypes 18 | void print_scan_status(uint8_t target, bool all); 19 | 20 | void setup() 21 | { 22 | pinMode(LED_BUILTIN,OUTPUT); // LED 23 | pinMode(12,INPUT_PULLUP); // Control for Test1 24 | pinMode(11,INPUT_PULLUP); // Control for Test2 25 | 26 | Serial.begin(115200); 27 | 28 | // Setup for Master mode, pins 18/19, external pullups, 400kHz 29 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 30 | } 31 | 32 | void loop() 33 | { 34 | uint8_t target; // slave addr 35 | bool all; 36 | 37 | // 38 | // Test1/2 - scan I2C addresses 39 | // - pull pin 12 low to show ACK only results 40 | // - pull pin 11 low for a little more verbose result (shows both ACK and NACK) 41 | // 42 | if(digitalRead(12) == LOW || digitalRead(11) == LOW) 43 | { 44 | all = (digitalRead(11) == LOW); 45 | Serial.print("---------------------------------------------------\n"); 46 | Serial.print("Starting scan...\n"); 47 | digitalWrite(LED_BUILTIN,HIGH); // LED on 48 | for(target = 1; target <= 0x7F; target++) // sweep addr, skip general call 49 | { 50 | Wire.beginTransmission(target); // slave addr 51 | Wire.endTransmission(); // no data, just addr 52 | print_scan_status(target, all); 53 | } 54 | digitalWrite(LED_BUILTIN,LOW); // LED off 55 | Serial.print("---------------------------------------------------\n"); 56 | 57 | delay(500); // delay to space out tests 58 | } 59 | } 60 | 61 | // 62 | // print scan status 63 | // 64 | void print_scan_status(uint8_t target, bool all) 65 | { 66 | switch(Wire.status()) 67 | { 68 | case I2C_WAITING: 69 | Serial.print("Addr 0x"); 70 | Serial.print(target,HEX); 71 | Serial.print(" ACK\n"); 72 | break; 73 | case I2C_ADDR_NAK: 74 | if(all) 75 | { 76 | Serial.print("Addr 0x"); 77 | Serial.print(target,HEX); 78 | Serial.print("\n"); 79 | } 80 | break; 81 | default: 82 | break; 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/slave/slave.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Slave 3 | // 08Mar13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C slave device with simple read/write commands and a small 7 | // addressable memory. 8 | // 9 | // This example code is in the public domain. 10 | // 11 | // ------------------------------------------------------------------------------------------- 12 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 13 | // a memory address to store to, and a sequence of data to store. 14 | // The command sequence is: 15 | // 16 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 17 | // 18 | // where START = I2C START sequence 19 | // I2CADDR+W = I2C Slave address + I2C write flag 20 | // WRITE = WRITE command 21 | // MEMADDR = memory address to store data to 22 | // DATAx = data byte to store, multiple bytes are stored to increasing address 23 | // STOP = I2C STOP sequence 24 | // ------------------------------------------------------------------------------------------- 25 | // READ - The I2C Master can read data from the device by transmitting the READ command, 26 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 27 | // followed by reading the data. The command sequence is: 28 | // 29 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 30 | // 31 | // where START = I2C START sequence 32 | // I2CADDR+W = I2C Slave address + I2C write flag 33 | // READ = READ command 34 | // MEMADDR = memory address to read data from 35 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 36 | // I2CADDR+R = I2C Slave address + I2C read flag 37 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 38 | // STOP = I2C STOP sequence 39 | // ------------------------------------------------------------------------------------------- 40 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 41 | // The command sequence is: 42 | // 43 | // START|I2CADDR+W|SETRATE|RATE|STOP 44 | // 45 | // where START = I2C START sequence 46 | // I2CADDR+W = I2C Slave address + I2C write flag 47 | // SETRATE = SETRATE command 48 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 49 | // ------------------------------------------------------------------------------------------- 50 | 51 | #include 52 | 53 | // Command definitions 54 | #define WRITE 0x10 55 | #define READ 0x20 56 | #define SETRATE 0x30 57 | 58 | // Function prototypes 59 | void receiveEvent(size_t len); 60 | void requestEvent(void); 61 | 62 | // Memory 63 | #define MEM_LEN 256 64 | uint8_t mem[MEM_LEN]; 65 | uint8_t cmd; 66 | size_t addr; 67 | i2c_rate rate; 68 | 69 | // 70 | // Setup 71 | // 72 | void setup() 73 | { 74 | pinMode(LED_BUILTIN,OUTPUT); // LED 75 | 76 | // Setup for Slave mode, address 0x44, pins 18/19, external pullups, 400kHz 77 | Wire.begin(I2C_SLAVE, 0x44, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 78 | 79 | // init vars 80 | cmd = 0; 81 | addr = 0; 82 | memset(mem, 0, sizeof(mem)); 83 | rate = I2C_RATE_400; 84 | 85 | // register events 86 | Wire.onReceive(receiveEvent); 87 | Wire.onRequest(requestEvent); 88 | 89 | Serial.begin(115200); 90 | } 91 | 92 | void loop() 93 | { 94 | digitalWrite(LED_BUILTIN,HIGH); // double pulse LED while waiting for I2C requests 95 | delay(10); // if the LED stops the slave is probably stuck in an ISR 96 | digitalWrite(LED_BUILTIN,LOW); 97 | delay(100); 98 | digitalWrite(LED_BUILTIN,HIGH); 99 | delay(10); 100 | digitalWrite(LED_BUILTIN,LOW); 101 | delay(880); 102 | } 103 | 104 | // 105 | // handle Rx Event (incoming I2C request/data) 106 | // 107 | void receiveEvent(size_t len) 108 | { 109 | if(Wire.available()) 110 | { 111 | // grab command 112 | cmd = Wire.readByte(); 113 | switch(cmd) 114 | { 115 | case WRITE: 116 | addr = Wire.readByte(); // grab addr 117 | while(Wire.available()) 118 | if(addr < MEM_LEN) // drop data beyond mem boundary 119 | mem[addr++] = Wire.readByte(); // copy data to mem 120 | else 121 | Wire.readByte(); // drop data if mem full 122 | break; 123 | 124 | case READ: 125 | addr = Wire.readByte(); // grab addr 126 | break; 127 | 128 | case SETRATE: 129 | rate = (i2c_rate)Wire.readByte(); // grab rate 130 | Wire.setRate(rate); // set rate 131 | break; 132 | } 133 | } 134 | } 135 | 136 | // 137 | // handle Tx Event (outgoing I2C data) 138 | // 139 | void requestEvent(void) 140 | { 141 | switch(cmd) 142 | { 143 | case READ: 144 | Wire.write(&mem[addr], MEM_LEN-addr); // fill Tx buffer (from addr location to end of mem) 145 | break; 146 | } 147 | } 148 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/slave_range/slave_range.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Slave w/Address Range 3 | // 08Mar13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C slave device which will respond to a range of I2C addresses. It will 7 | // dump any data it receives to Serial. 8 | // 9 | // The getRxAddr() function can be used to obtain the Slave address in the callback. This 10 | // could then be used to alter Slave response and make a single device then appear as multiple 11 | // different Slave targets. 12 | // 13 | // This example code is in the public domain. 14 | // ------------------------------------------------------------------------------------------- 15 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 16 | // a memory address to store to, and a sequence of data to store. 17 | // The command sequence is: 18 | // 19 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 20 | // 21 | // where START = I2C START sequence 22 | // I2CADDR+W = I2C Slave address + I2C write flag 23 | // WRITE = WRITE command 24 | // MEMADDR = memory address to store data to 25 | // DATAx = data byte to store, multiple bytes are stored to increasing address 26 | // STOP = I2C STOP sequence 27 | // ------------------------------------------------------------------------------------------- 28 | // READ - The I2C Master can read data from the device by transmitting the READ command, 29 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 30 | // followed by reading the data. The command sequence is: 31 | // 32 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 33 | // 34 | // where START = I2C START sequence 35 | // I2CADDR+W = I2C Slave address + I2C write flag 36 | // READ = READ command 37 | // MEMADDR = memory address to read data from 38 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 39 | // I2CADDR+R = I2C Slave address + I2C read flag 40 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 41 | // STOP = I2C STOP sequence 42 | // ------------------------------------------------------------------------------------------- 43 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 44 | // The command sequence is: 45 | // 46 | // START|I2CADDR+W|SETRATE|RATE|STOP 47 | // 48 | // where START = I2C START sequence 49 | // I2CADDR+W = I2C Slave address + I2C write flag 50 | // SETRATE = SETRATE command 51 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 52 | // ------------------------------------------------------------------------------------------- 53 | 54 | #include 55 | 56 | // Command definitions 57 | #define WRITE 0x10 58 | #define READ 0x20 59 | #define SETRATE 0x30 60 | 61 | // Function prototypes 62 | void receiveEvent(size_t len); 63 | void requestEvent(void); 64 | 65 | // Memory 66 | #define MEM_LEN 256 67 | uint8_t mem[MEM_LEN]; 68 | uint8_t cmd, data, target; 69 | size_t addr; 70 | i2c_rate rate; 71 | 72 | // 73 | // Setup 74 | // 75 | void setup() 76 | { 77 | pinMode(LED_BUILTIN,OUTPUT); // LED 78 | 79 | Serial.begin(115200); 80 | 81 | // Setup for Slave mode, addresses 0x08 to 0x77, pins 18/19, external pullups, 400kHz 82 | Wire.begin(I2C_SLAVE, 0x08, 0x77, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 83 | 84 | // init vars 85 | cmd = 0; 86 | addr = 0; 87 | memset(mem, 0, sizeof(mem)); 88 | rate = I2C_RATE_400; 89 | 90 | // register events 91 | Wire.onReceive(receiveEvent); 92 | Wire.onRequest(requestEvent); 93 | } 94 | 95 | void loop() 96 | { 97 | digitalWrite(LED_BUILTIN,HIGH); // double pulse LED while waiting for I2C requests 98 | delay(10); // if the LED stops the slave is probably stuck in an ISR 99 | digitalWrite(LED_BUILTIN,LOW); 100 | delay(100); 101 | digitalWrite(LED_BUILTIN,HIGH); 102 | delay(10); 103 | digitalWrite(LED_BUILTIN,LOW); 104 | delay(880); 105 | } 106 | 107 | // 108 | // handle Rx Event (incoming I2C request/data) 109 | // 110 | void receiveEvent(size_t len) 111 | { 112 | if(Wire.available()) 113 | { 114 | // grab command 115 | target = Wire.getRxAddr(); // getRxAddr() is used to obtain Slave address 116 | cmd = Wire.readByte(); 117 | switch(cmd) 118 | { 119 | case WRITE: 120 | addr = Wire.readByte(); // grab mem addr 121 | 122 | Serial.printf("WRITE to Slave 0x%0X at MemAddr: %d Data: ",target,addr); 123 | while(Wire.available()) 124 | if(addr < MEM_LEN) // drop data beyond mem boundary 125 | { 126 | data = Wire.readByte(); // grab data 127 | mem[addr++] = data; // copy data to mem 128 | Serial.printf("%d ",data); 129 | } 130 | else 131 | Wire.readByte(); // drop data if mem full 132 | Serial.print("\n"); 133 | break; 134 | 135 | case READ: 136 | target = Wire.getRxAddr(); 137 | addr = Wire.readByte(); // grab addr 138 | Serial.printf("READ from Slave 0x%0X at MemAddr: %d\n",target,addr); 139 | break; 140 | 141 | case SETRATE: 142 | rate = (i2c_rate)Wire.readByte(); // grab rate 143 | Wire.setRate(rate); // set rate 144 | Serial.print("Rate changed\n"); 145 | break; 146 | } 147 | } 148 | } 149 | 150 | // 151 | // handle Tx Event (outgoing I2C data) 152 | // 153 | void requestEvent(void) 154 | { 155 | switch(cmd) 156 | { 157 | case READ: 158 | Wire.write(&mem[addr], MEM_LEN-addr); // fill Tx buffer (from addr location to end of mem) 159 | break; 160 | } 161 | } 162 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/examples/timeout/timeout.ino: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------- 2 | // Teensy3.0/3.1/LC I2C Timeout Test 3 | // 15May13 Brian (nox771 at gmail.com) 4 | // ------------------------------------------------------------------------------------------- 5 | // 6 | // This creates an I2C master device which talks to the simple I2C slave device given in the 7 | // i2c_t3/slave sketch. It tests the various timeout functions. 8 | // 9 | // This code assumes the slave config has >8byte memory and I2C addr is 0x44. 10 | // The test is started by pulling the control pin low. 11 | // 12 | // This example code is in the public domain. 13 | // 14 | // ------------------------------------------------------------------------------------------- 15 | // Slave protocol is as follows: 16 | // ------------------------------------------------------------------------------------------- 17 | // WRITE - The I2C Master can write to the device by transmitting the WRITE command, 18 | // a memory address to store to, and a sequence of data to store. 19 | // The command sequence is: 20 | // 21 | // START|I2CADDR+W|WRITE|MEMADDR|DATA0|DATA1|DATA2|...|STOP 22 | // 23 | // where START = I2C START sequence 24 | // I2CADDR+W = I2C Slave address + I2C write flag 25 | // WRITE = WRITE command 26 | // MEMADDR = memory address to store data to 27 | // DATAx = data byte to store, multiple bytes are stored to increasing address 28 | // STOP = I2C STOP sequence 29 | // ------------------------------------------------------------------------------------------- 30 | // READ - The I2C Master can read data from the device by transmitting the READ command, 31 | // a memory address to read from, and then issuing a STOP/START or Repeated-START, 32 | // followed by reading the data. The command sequence is: 33 | // 34 | // START|I2CADDR+W|READ|MEMADDR|REPSTART|I2CADDR+R|DATA0|DATA1|DATA2|...|STOP 35 | // 36 | // where START = I2C START sequence 37 | // I2CADDR+W = I2C Slave address + I2C write flag 38 | // READ = READ command 39 | // MEMADDR = memory address to read data from 40 | // REPSTART = I2C Repeated-START sequence (or STOP/START if single Master) 41 | // I2CADDR+R = I2C Slave address + I2C read flag 42 | // DATAx = data byte read by Master, multiple bytes are read from increasing address 43 | // STOP = I2C STOP sequence 44 | // ------------------------------------------------------------------------------------------- 45 | // SETRATE - The I2C Master can adjust the Slave configured I2C rate with this command 46 | // The command sequence is: 47 | // 48 | // START|I2CADDR+W|SETRATE|RATE|STOP 49 | // 50 | // where START = I2C START sequence 51 | // I2CADDR+W = I2C Slave address + I2C write flag 52 | // SETRATE = SETRATE command 53 | // RATE = I2C RATE to use (must be from i2c_rate enum list, eg. I2C_RATE_xxxx) 54 | // ------------------------------------------------------------------------------------------- 55 | 56 | #include 57 | 58 | // Command definitions 59 | #define WRITE 0x10 60 | #define READ 0x20 61 | #define SETRATE 0x30 62 | 63 | // Function prototypes 64 | void print_i2c_status(void); 65 | 66 | void setup() 67 | { 68 | pinMode(LED_BUILTIN,OUTPUT); // LED 69 | pinMode(12,INPUT_PULLUP); // Control for Test sequence 70 | 71 | // Setup for Master mode, pins 18/19, external pullups, 400kHz 72 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); 73 | 74 | Serial.begin(115200); 75 | } 76 | 77 | void loop() 78 | { 79 | uint8_t target = 0x44; 80 | size_t addr, len; 81 | uint8_t databuf[256]; 82 | elapsedMicros deltaT; 83 | uint32_t hold; 84 | 85 | if(digitalRead(12) == LOW) 86 | { 87 | digitalWrite(LED_BUILTIN,HIGH); // LED on 88 | Serial.print("---------------------------------------------------------\n"); 89 | Serial.print("Test1 : Blocking transmit with 50us timeout\n"); 90 | Serial.print("---------------------------------------------------------\n"); 91 | 92 | // Writing to Slave -------------------------------------------------------- 93 | addr = 0; 94 | for(len = 0; len < 8; len++) 95 | databuf[len] = len; // prepare data, set data == address 96 | Serial.printf("I2C WRITE 8 bytes to Slave 0x%0X at MemAddr %d (50us timeout)\n", target, addr); 97 | 98 | Wire.beginTransmission(target); // slave addr 99 | Wire.write(WRITE); // WRITE command 100 | Wire.write(addr); // memory address 101 | for(len = 0; len < 8; len++) // write 8 byte block 102 | Wire.write(databuf[len]); 103 | deltaT = 0; 104 | Wire.endTransmission(I2C_STOP,50); // blocking Tx with 50us timeout 105 | hold = deltaT; 106 | 107 | print_i2c_status(); // print I2C final status 108 | Serial.printf("Duration: %dus\n", hold); // print duration 109 | delay(1); // delay to space out tests 110 | 111 | Serial.print("---------------------------------------------------------\n"); 112 | Serial.print("Test2 : Blocking transmit with no timeout\n"); 113 | Serial.print("---------------------------------------------------------\n"); 114 | Serial.printf("I2C WRITE 8 bytes to Slave 0x%0X at MemAddr %d (no timeout)\n", target, addr); 115 | 116 | Wire.beginTransmission(target); // slave addr 117 | Wire.write(WRITE); // WRITE command 118 | Wire.write(addr); // memory address 119 | for(len = 0; len < 8; len++) // write 8 byte block 120 | Wire.write(databuf[len]); 121 | deltaT = 0; 122 | Wire.endTransmission(I2C_STOP); // blocking Tx with no timeout 123 | hold = deltaT; 124 | 125 | print_i2c_status(); // print I2C final status 126 | Serial.printf("Duration: %dus\n", hold); // print duration 127 | delay(1); // delay to space out tests 128 | 129 | Serial.print("---------------------------------------------------------\n"); 130 | Serial.print("Test3 : Blocking receive with 50us timeout\n"); 131 | Serial.print("---------------------------------------------------------\n"); 132 | Serial.printf("I2C READ 8 bytes from Slave 0x%0X at MemAddr %d (50us timeout)\n", target, addr); 133 | Serial.print("Note: resulting timeout duration is longer since this takes 2 commands WRITE then READ\n"); 134 | 135 | Wire.beginTransmission(target); // slave addr 136 | Wire.write(READ); // WRITE command 137 | Wire.write(addr); // memory address 138 | deltaT = 0; 139 | Wire.endTransmission(I2C_NOSTOP); // blocking Tx, no STOP 140 | Wire.requestFrom(target,8,I2C_STOP,50); // blocking Rx with 50us timeout 141 | hold = deltaT; 142 | 143 | print_i2c_status(); // print I2C final status 144 | Serial.printf("Duration: %dus\n", hold); // print duration 145 | delay(1); // delay to space out tests 146 | 147 | Serial.print("---------------------------------------------------------\n"); 148 | Serial.print("Test4 : Blocking receive with no timeout\n"); 149 | Serial.print("---------------------------------------------------------\n"); 150 | Serial.printf("I2C READ 8 bytes from Slave 0x%0X at MemAddr %d (no timeout)\n", target, addr); 151 | 152 | Wire.beginTransmission(target); // slave addr 153 | Wire.write(READ); // WRITE command 154 | Wire.write(addr); // memory address 155 | deltaT = 0; 156 | Wire.endTransmission(I2C_NOSTOP); // blocking Tx, no STOP 157 | Wire.requestFrom(target,8,I2C_STOP); // blocking Rx with no timeout 158 | hold = deltaT; 159 | 160 | print_i2c_status(); // print I2C final status 161 | Serial.printf("Duration: %dus\n", hold); // print duration 162 | delay(1); // delay to space out tests 163 | 164 | Serial.print("---------------------------------------------------------\n"); 165 | Serial.print("Test5 : Non-blocking transmit with 50us finish() timeout\n"); 166 | Serial.print("---------------------------------------------------------\n"); 167 | Serial.printf("I2C WRITE 8 bytes to Slave 0x%0X at MemAddr %d (50us finish() timeout)\n", target, addr); 168 | 169 | Wire.beginTransmission(target); // slave addr 170 | Wire.write(WRITE); // WRITE command 171 | Wire.write(addr); // memory address 172 | for(len = 0; len < 8; len++) // write 8 byte block 173 | Wire.write(databuf[len]); 174 | deltaT = 0; 175 | Wire.sendTransmission(I2C_STOP); // non-blocking Tx 176 | // 177 | // I2C working in background, so do other stuff here 178 | // 179 | Wire.finish(50); // finish with timeout 180 | hold = deltaT; 181 | 182 | print_i2c_status(); // print I2C final status 183 | Serial.printf("Duration: %dus\n", hold); // print duration 184 | delay(1); // delay to space out tests 185 | 186 | Serial.print("Done\n"); 187 | digitalWrite(LED_BUILTIN,LOW); // LED on 188 | delay(1000); // delay to space out tests 189 | } 190 | } 191 | 192 | // 193 | // print I2C status 194 | // 195 | void print_i2c_status(void) 196 | { 197 | switch(Wire.status()) 198 | { 199 | case I2C_WAITING: Serial.print("I2C waiting, no errors\n"); break; 200 | case I2C_ADDR_NAK: Serial.print("Slave addr not acknowledged\n"); break; 201 | case I2C_DATA_NAK: Serial.print("Slave data not acknowledged\n"); break; 202 | case I2C_ARB_LOST: Serial.print("Bus Error: Arbitration Lost\n"); break; 203 | case I2C_TIMEOUT: Serial.print("I2C timeout\n"); break; 204 | case I2C_BUF_OVF: Serial.print("I2C buffer overflow\n"); break; 205 | default: Serial.print("I2C busy\n"); break; 206 | } 207 | } 208 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/keywords.txt: -------------------------------------------------------------------------------- 1 | I2C_BUS_NUM LITERAL1 2 | I2C_TX_BUFFER_LENGTH LITERAL1 3 | I2C_RX_BUFFER_LENGTH LITERAL1 4 | I2C_OP_MODE_IMM LITERAL1 5 | I2C_OP_MODE_ISR LITERAL1 6 | I2C_OP_MODE_DMA LITERAL1 7 | I2C_MASTER LITERAL1 8 | I2C_SLAVE LITERAL1 9 | I2C_PINS_18_19 LITERAL1 10 | I2C_PINS_16_17 LITERAL1 11 | I2C_PINS_22_23 LITERAL1 12 | I2C_PINS_29_30 LITERAL1 13 | I2C_PINS_26_31 LITERAL1 14 | I2C_PULLUP_EXT LITERAL1 15 | I2C_PULLUP_INT LITERAL1 16 | I2C_RATE_100 LITERAL1 17 | I2C_RATE_200 LITERAL1 18 | I2C_RATE_300 LITERAL1 19 | I2C_RATE_400 LITERAL1 20 | I2C_RATE_600 LITERAL1 21 | I2C_RATE_800 LITERAL1 22 | I2C_RATE_1000 LITERAL1 23 | I2C_RATE_1200 LITERAL1 24 | I2C_RATE_1500 LITERAL1 25 | I2C_RATE_1800 LITERAL1 26 | I2C_RATE_2000 LITERAL1 27 | I2C_RATE_2400 LITERAL1 28 | I2C_RATE_2800 LITERAL1 29 | I2C_RATE_3000 LITERAL1 30 | I2C_NOSTOP LITERAL1 31 | I2C_STOP LITERAL1 32 | I2C_WAITING LITERAL1 33 | I2C_SENDING LITERAL1 34 | I2C_SEND_ADDR LITERAL1 35 | I2C_RECEIVING LITERAL1 36 | I2C_TIMEOUT LITERAL1 37 | I2C_ADDR_NAK LITERAL1 38 | I2C_DATA_NAK LITERAL1 39 | I2C_ARB_LOST LITERAL1 40 | I2C_BUF_OVF LITERAL1 41 | I2C_SLAVE_TX LITERAL1 42 | I2C_SLAVE_RX LITERAL1 43 | 44 | Wire KEYWORD2 45 | Wire1 KEYWORD2 46 | i2c_t3 KEYWORD2 47 | begin KEYWORD2 48 | setOpMode KEYWORD2 49 | setRate KEYWORD2 50 | setClock KEYWORD2 51 | pinConfigure KEYWORD2 52 | setDefaultTimeout KEYWORD2 53 | resetBus KEYWORD2 54 | beginTransmission KEYWORD2 55 | endTransmission KEYWORD2 56 | sendTransmission KEYWORD2 57 | requestFrom KEYWORD2 58 | sendRequest KEYWORD2 59 | getError KEYWORD2 60 | status KEYWORD2 61 | done KEYWORD2 62 | finish KEYWORD2 63 | write KEYWORD2 64 | available KEYWORD2 65 | read KEYWORD2 66 | peek KEYWORD2 67 | readByte KEYWORD2 68 | peekByte KEYWORD2 69 | getRxAddr KEYWORD2 70 | onReceive KEYWORD2 71 | onRequest KEYWORD2 72 | send KEYWORD2 73 | receive KEYWORD2 74 | -------------------------------------------------------------------------------- /firmware/lib/i2c_t3/speedtest.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Poofjunior/imu_noodle/6a9538ec2ad86af870b3ac77f49af03c439fb0ac/firmware/lib/i2c_t3/speedtest.jpg -------------------------------------------------------------------------------- /firmware/lib/i2c_wrapper/i2c_wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "i2c_wrapper.h" 2 | 3 | #define TIMEOUT_MS 20 4 | 5 | void i2cInit() 6 | { 7 | /* 8 | /// Initialize I2C for Teensy 3 as master (with dummy slave address). 9 | uint8_t dummyAddress = 0x00; 10 | Wire.begin(I2C_MASTER, dummyAddress, I2C_PINS_18_19, I2C_PULLUP_EXT, 11 | I2C_RATE_400); 12 | */ 13 | Wire.begin(); 14 | } 15 | 16 | uint8_t setStartingRegisterAddress(const uint8_t device_address, 17 | const uint8_t register_address) 18 | { 19 | Wire.beginTransmission(device_address); 20 | Wire.write(register_address); 21 | return Wire.endTransmission(); 22 | } 23 | 24 | uint8_t i2cReadByte(const uint8_t device_address, const uint8_t register_address, 25 | uint8_t& value) 26 | { 27 | const uint8_t num_bytes = 1; 28 | uint8_t temp_array[num_bytes]; 29 | uint8_t status_byte = i2cRead(device_address, register_address, 30 | num_bytes, temp_array); 31 | value = temp_array[0]; 32 | return status_byte; 33 | } 34 | 35 | uint8_t i2cWriteByte(const uint8_t device_address, const uint8_t register_address, 36 | uint8_t value) 37 | { 38 | uint8_t num_bytes = 1; 39 | uint8_t temp_array[num_bytes]; 40 | temp_array[0] = value; 41 | return i2cWrite(device_address, register_address, num_bytes, temp_array); 42 | } 43 | 44 | uint8_t i2cRead(const uint8_t device_address, const uint8_t register_address, 45 | const uint8_t num_bytes, uint8_t * const arrayPtr) 46 | { 47 | uint8_t status_byte = setStartingRegisterAddress(device_address, register_address); 48 | if (!status_byte) 49 | { 50 | Wire.requestFrom(device_address, num_bytes); 51 | for (uint8_t data_index = 0; data_index < num_bytes; ++data_index) 52 | { 53 | int t = millis(); 54 | while((!Wire.available()) && (millis() < t+TIMEOUT_MS)); 55 | arrayPtr[data_index] = Wire.read(); 56 | } 57 | } 58 | return status_byte; 59 | } 60 | 61 | uint8_t i2cWrite(const uint8_t device_address, const uint8_t register_address, 62 | const uint8_t num_bytes, uint8_t const * arrayPtr) 63 | { 64 | Wire.beginTransmission(device_address); 65 | Wire.write(register_address); 66 | for (uint8_t data_index = 0; data_index < num_bytes; ++data_index) 67 | { 68 | Wire.write(arrayPtr[data_index]); 69 | } 70 | return Wire.endTransmission(); 71 | } 72 | 73 | /*** 16-bit register addresses ***/ 74 | uint8_t setStartingRegisterAddress(const uint8_t device_address, 75 | const uint16_t register_address) 76 | { 77 | Wire.beginTransmission(device_address); 78 | Wire.write(uint8_t(0xFF & (register_address >> 8))); 79 | Wire.write(uint8_t(0xFF & register_address)); 80 | return Wire.endTransmission(); 81 | } 82 | 83 | uint8_t i2cReadByte(const uint8_t device_address, const uint16_t register_address, 84 | uint8_t& value) 85 | { 86 | const uint8_t num_bytes = 1; 87 | uint8_t temp_array[num_bytes]; 88 | uint8_t status_byte = i2cRead(device_address, register_address, 89 | num_bytes, temp_array); 90 | value = temp_array[0]; 91 | return status_byte; 92 | } 93 | 94 | uint8_t i2cWriteByte(const uint8_t device_address, const uint16_t register_address, 95 | uint8_t value) 96 | { 97 | uint8_t num_bytes = 1; 98 | uint8_t temp_array[num_bytes]; 99 | temp_array[0] = value; 100 | return i2cWrite(device_address, register_address, num_bytes, temp_array); 101 | } 102 | 103 | uint8_t i2cRead(const uint8_t device_address, const uint16_t register_address, 104 | const uint8_t num_bytes, uint8_t * const arrayPtr) 105 | { 106 | uint8_t status_byte = setStartingRegisterAddress(device_address, register_address); 107 | if (!status_byte) 108 | { 109 | Wire.requestFrom(device_address, num_bytes); 110 | for (uint8_t data_index = 0; data_index < num_bytes; ++data_index) 111 | { 112 | int t = millis(); 113 | while((!Wire.available()) && (millis() < t+TIMEOUT_MS)); 114 | arrayPtr[data_index] = Wire.read(); 115 | } 116 | } 117 | return status_byte; 118 | } 119 | 120 | uint8_t i2cWrite(const uint8_t device_address, const uint16_t register_address, 121 | const uint8_t num_bytes, uint8_t const * arrayPtr) 122 | { 123 | Wire.beginTransmission(device_address); 124 | Wire.write(uint8_t(0xFF & (register_address >> 8))); 125 | Wire.write(register_address); 126 | for (uint8_t data_index = 0; data_index < num_bytes; ++data_index) 127 | { 128 | Wire.write(arrayPtr[data_index]); 129 | } 130 | return Wire.endTransmission(); 131 | } 132 | 133 | -------------------------------------------------------------------------------- /firmware/lib/i2c_wrapper/i2c_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_WRAPPER_HPP_ 2 | #define I2C_WRAPPER_HPP_ 3 | #include 4 | #include 5 | #include 6 | 7 | void i2cInit(); 8 | 9 | /// 8-bit register address versions 10 | /** 11 | * \brief for i2c peripherals that follow the 12 | WRITE_START STOP 13 | WRITE_START ... STOP 14 | paradigm OR 15 | WRITE_START STOP 16 | READ_START ... STOP 17 | this fn writes the starting register_address to the specified device_address 18 | */ 19 | uint8_t setStartingRegisterAddress(const uint8_t device_address, 20 | const uint8_t register_address); 21 | 22 | uint8_t i2cReadByte(const uint8_t device_address, const uint8_t mem_address, 23 | uint8_t& value); 24 | 25 | uint8_t i2cWriteByte(const uint8_t device_address, const uint8_t mem_address, 26 | uint8_t value); 27 | 28 | uint8_t i2cRead(const uint8_t device_address, const uint8_t mem_address, 29 | const uint8_t num_bytes, uint8_t * const arrayPtr); 30 | 31 | uint8_t i2cWrite(const uint8_t device_address, const uint8_t mem_address, 32 | const uint8_t num_bytes, uint8_t const * arrayPtr); 33 | 34 | /// 16-bit register address versions 35 | 36 | /** 37 | * \brief performs identically to setStartingRegisterAddress above except this fn 38 | * writes a 2-byte address. 39 | * \note for use with devices that have a larger register map that spans 40 | * an address space beyond the typical 8 bits. 41 | */ 42 | uint8_t setStartingRegisterAddress(const uint8_t device_address, 43 | const uint16_t mem_address); 44 | 45 | uint8_t i2cReadByte(const uint8_t device_address, const uint16_t mem_address, 46 | uint8_t& value); 47 | 48 | uint8_t i2cWriteByte(const uint8_t device_address, const uint16_t mem_address, 49 | uint8_t value); 50 | 51 | uint8_t i2cRead(const uint8_t device_address, const uint16_t mem_address, 52 | const uint8_t num_bytes, uint8_t * const arrayPtr); 53 | 54 | uint8_t i2cWrite(const uint8_t device_address, const uint16_t mem_address, 55 | const uint8_t num_bytes, uint8_t const * arrayPtr); 56 | #endif /// I2C_WRAPPER_HPP_ 57 | -------------------------------------------------------------------------------- /firmware/multi_node_bringup/Makefile: -------------------------------------------------------------------------------- 1 | # Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile 2 | #The mobile embedded interface poses as a generic 3 | 4 | GIT_HASH = "\"$(shell git rev-parse HEAD)\"" 5 | GIT_BRANCH = "\"$(shell git branch | sed -n -e 's/^\* \(.*\)/\1/p')\"" 6 | UPLOAD_DATE = "\"$(shell date)\"" 7 | 8 | CXXFLAGS_STD = -std=gnu++11 -DGIT_HASH=$(GIT_HASH) -DGIT_BRANCH=$(GIT_BRANCH) \ 9 | -DUPLOAD_DATE=$(UPLOAD_DATE) 10 | 11 | USB_TYPE = USB_SERIAL 12 | 13 | # WARNING: Trailing whitespace will cause options to be read incorrectly. 14 | BOARD_TAG = teensy31 15 | 16 | # Additional user-defined libraries may be added to this list 17 | ARDUINO_LIBS += bno055 \ 18 | i2c_wrapper \ 19 | i2c_t3 20 | 21 | # Default user-defined libraries directory 22 | USER_LIB_PATH += ../lib 23 | 24 | # Teensy explicitly requires Arduino 1.0.x as of Nov 2014 25 | # ARDUINO_DIR should be defined in your Makefile. Otherwise, you may 26 | # define it here. 27 | 28 | # teensy3, teensy31, and teensy32 explicitly require their own Makefile. 29 | include $(ARDMK_DIR)/Teensy.mk 30 | 31 | -------------------------------------------------------------------------------- /firmware/multi_node_bringup/multi_node_bringup.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * \project multi_board_bringup.ino 3 | * \author Joshua Vasquez 4 | */ 5 | 6 | #include "pinouts.h" 7 | #include 8 | #include 9 | 10 | const uint8_t NUM_BNOS = 5; 11 | BNO055 bno_array[NUM_BNOS]; 12 | uint8_t addresses[NUM_BNOS] = {0x2F, 0x2D, 0x2E, 0x2C, BNO055::ALTERNATE_ADDRESS}; 13 | //uint8_t addresses[NUM_BNOS] = {0x2E, 0x2D}; 14 | //uint8_t addresses[NUM_BNOS] = {0x2A}; 15 | 16 | void setup(){ 17 | Serial.begin(9600);// baud rate irrelevant for usb serial. 18 | delay(3000); 19 | Serial.print("Git Hash: "); 20 | Serial.println(GIT_HASH); 21 | Serial.print("Git Branch: "); 22 | Serial.println(GIT_BRANCH); 23 | Serial.print("Upload Date: "); 24 | Serial.println(UPLOAD_DATE); 25 | i2cInit(); 26 | delay(1000); 27 | for (uint8_t index = 0; index < NUM_BNOS; ++index) 28 | { 29 | if(!bno_array[index].init(BNO055::NDOF, (BNO055::address_t)addresses[index])) 30 | { 31 | Serial.print("BNO055 "); 32 | Serial.print(index); 33 | Serial.println(" init succeeded."); 34 | } 35 | else 36 | { 37 | Serial.print("BNO055 "); 38 | Serial.print(index); 39 | Serial.println(" init failed."); 40 | } 41 | } 42 | delay(2000); 43 | } 44 | 45 | void loop(void) 46 | { 47 | float heading; 48 | int16_t w, x, y, z; 49 | float QUAT_BITS_TO_FLOAT = 16384.0; 50 | for (uint8_t index = 0; index < NUM_BNOS; ++index) 51 | { 52 | heading = bno_array[index].readRawQuaternion(w, x, y, z); 53 | Serial.print(index); 54 | Serial.print(": "); 55 | Serial.print("w: "); 56 | Serial.print(w/QUAT_BITS_TO_FLOAT); 57 | Serial.print(" x: "); 58 | Serial.print(x/QUAT_BITS_TO_FLOAT); 59 | Serial.print(" y: "); 60 | Serial.print(y/QUAT_BITS_TO_FLOAT); 61 | Serial.print(" z: "); 62 | Serial.println(z/QUAT_BITS_TO_FLOAT); 63 | } 64 | Serial.println(); 65 | delay(50); 66 | } 67 | -------------------------------------------------------------------------------- /firmware/multi_node_bringup/pinouts.h: -------------------------------------------------------------------------------- 1 | #ifndef PINOUTS_H 2 | #define PINOUTS_H 3 | 4 | 5 | #endif // PINOUTS_H 6 | -------------------------------------------------------------------------------- /firmware/single_node_bringup/Makefile: -------------------------------------------------------------------------------- 1 | # Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile 2 | #The mobile embedded interface poses as a generic 3 | 4 | GIT_HASH = "\"$(shell git rev-parse HEAD)\"" 5 | GIT_BRANCH = "\"$(shell git branch | sed -n -e 's/^\* \(.*\)/\1/p')\"" 6 | UPLOAD_DATE = "\"$(shell date)\"" 7 | 8 | CXXFLAGS_STD = -std=gnu++11 -DGIT_HASH=$(GIT_HASH) -DGIT_BRANCH=$(GIT_BRANCH) \ 9 | -DUPLOAD_DATE=$(UPLOAD_DATE) 10 | 11 | USB_TYPE = USB_SERIAL 12 | 13 | # WARNING: Trailing whitespace will cause options to be read incorrectly. 14 | BOARD_TAG = teensy31 15 | 16 | # Additional user-defined libraries may be added to this list 17 | ARDUINO_LIBS += bno055 \ 18 | i2c_wrapper \ 19 | i2c_t3 20 | 21 | # Default user-defined libraries directory 22 | USER_LIB_PATH += ../lib 23 | 24 | # Teensy explicitly requires Arduino 1.0.x as of Nov 2014 25 | # ARDUINO_DIR should be defined in your Makefile. Otherwise, you may 26 | # define it here. 27 | 28 | # teensy3, teensy31, and teensy32 explicitly require their own Makefile. 29 | include $(ARDMK_DIR)/Teensy.mk 30 | 31 | -------------------------------------------------------------------------------- /firmware/single_node_bringup/pinouts.h: -------------------------------------------------------------------------------- 1 | #ifndef PINOUTS_H 2 | #define PINOUTS_H 3 | 4 | 5 | #endif // PINOUTS_H 6 | -------------------------------------------------------------------------------- /firmware/single_node_bringup/single_node_bringup.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * \project single_board_bringup.ino 3 | * \author Joshua Vasquez 4 | */ 5 | 6 | #include "pinouts.h" 7 | #include 8 | #include 9 | 10 | BNO055 bno055; 11 | BNO055::address_t address = (BNO055::address_t)0x22; 12 | 13 | void setup(){ 14 | Serial.begin(9600);// baud rate irrelevant for usb serial. 15 | delay(3000); 16 | Serial.print("Git hash: "); 17 | Serial.println(GIT_HASH); 18 | Serial.print("Git branch: "); 19 | Serial.println(GIT_BRANCH); 20 | Serial.print("Upload Date: "); 21 | Serial.println(UPLOAD_DATE); 22 | i2cInit(); 23 | delay(1000); 24 | if(!bno055.init(BNO055::NDOF, address)) 25 | Serial.println("BNO055 init succeeded."); 26 | else 27 | { 28 | Serial.print("BNO055 at "); 29 | Serial.print("0x"); 30 | Serial.print(address, HEX); 31 | Serial.println(" init failed."); 32 | } 33 | } 34 | 35 | void loop(void) 36 | { 37 | float heading = bno055.readEulHead()/900.0; 38 | Serial.println(heading); 39 | delay(50); 40 | } 41 | -------------------------------------------------------------------------------- /firmware/usb_node_device/Makefile: -------------------------------------------------------------------------------- 1 | # Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile 2 | #The mobile embedded interface poses as a generic 3 | 4 | GIT_HASH = "\"$(shell git rev-parse HEAD)\"" 5 | GIT_BRANCH = "\"$(shell git branch | sed -n -e 's/^\* \(.*\)/\1/p')\"" 6 | UPLOAD_DATE = "\"$(shell date)\"" 7 | 8 | CXXFLAGS_STD = -std=gnu++11 -DGIT_HASH=$(GIT_HASH) -DGIT_BRANCH=$(GIT_BRANCH) \ 9 | -DUPLOAD_DATE=$(UPLOAD_DATE) 10 | 11 | #USB_TYPE = USB_SERIAL 12 | USB_TYPE = USB_RAWHID 13 | 14 | # WARNING: Trailing whitespace will cause options to be read incorrectly. 15 | BOARD_TAG = teensy31 16 | 17 | # Additional user-defined libraries may be added to this list 18 | ARDUINO_LIBS += bno055 \ 19 | i2c_wrapper \ 20 | i2c_t3 \ 21 | usb_packet 22 | 23 | # Default user-defined libraries directory 24 | USER_LIB_PATH += ../lib 25 | 26 | # Teensy explicitly requires Arduino 1.0.x as of Nov 2014 27 | # ARDUINO_DIR should be defined in your Makefile. Otherwise, you may 28 | # define it here. 29 | 30 | # teensy3, teensy31, and teensy32 explicitly require their own Makefile. 31 | include $(ARDMK_DIR)/Teensy.mk 32 | 33 | -------------------------------------------------------------------------------- /firmware/usb_node_device/usb_node_device.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * \project usb_node_device.ino 3 | * \author Joshua Vasquez 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | const uint8_t NUM_BNOS = 5; 11 | BNO055 bno_array[NUM_BNOS]; 12 | uint8_t addresses[NUM_BNOS] = {0x2F, 0x2C, 0x2E, 0x2D, BNO055::ALTERNATE_ADDRESS}; 13 | 14 | USBPacket usbPacket; 15 | 16 | size_t sensorIndex = 0; 17 | 18 | void setup(){ 19 | i2cInit(); 20 | delay(1000); 21 | for (uint8_t index = 0; index < NUM_BNOS; ++index) 22 | { 23 | bno_array[index].init(BNO055::NDOF, (BNO055::address_t)addresses[index]); 24 | } 25 | } 26 | 27 | /// Continuously read all the BNO055s in order and output their 28 | /// data over usb. 29 | void loop(void) 30 | { 31 | int16_t w, x, y, z; 32 | USBPacket::quatPacket qPacket; 33 | /// Stuff as many quaternion packets into the usb packet per transfer. 34 | for (uint8_t packetIndex = 0; 35 | packetIndex < USBPacket::QUATERNIONS_PER_PACKET; ++packetIndex) 36 | { 37 | bno_array[sensorIndex].readRawQuaternion(w, x, y, z); 38 | qPacket.index = sensorIndex; 39 | qPacket.qData.w = w/float(BNO055::QUAT_BITS_TO_FLOAT); 40 | qPacket.qData.x = x/float(BNO055::QUAT_BITS_TO_FLOAT); 41 | qPacket.qData.y = y/float(BNO055::QUAT_BITS_TO_FLOAT); 42 | qPacket.qData.z = z/float(BNO055::QUAT_BITS_TO_FLOAT); 43 | 44 | usbPacket.setQuaternionPacket(packetIndex, qPacket); 45 | ++sensorIndex; 46 | if (sensorIndex == NUM_BNOS) 47 | { 48 | sensorIndex = 0; 49 | } 50 | } 51 | RawHID.send(usbPacket.packet_, 0); 52 | delay(6); 53 | } 54 | -------------------------------------------------------------------------------- /pcbs/di2c_breakout/di2c_breakout.pro: -------------------------------------------------------------------------------- 1 | update=Fri 20 Apr 2018 05:29:51 PM PDT 2 | version=1 3 | last_client=kicad 4 | [pcbnew] 5 | version=1 6 | LastNetListRead= 7 | UseCmpFile=1 8 | PadDrill=0.600000000000 9 | PadDrillOvalY=0.600000000000 10 | PadSizeH=1.500000000000 11 | PadSizeV=1.500000000000 12 | PcbTextSizeV=1.500000000000 13 | PcbTextSizeH=1.500000000000 14 | PcbTextThickness=0.300000000000 15 | ModuleTextSizeV=1.000000000000 16 | ModuleTextSizeH=1.000000000000 17 | ModuleTextSizeThickness=0.150000000000 18 | SolderMaskClearance=0.000000000000 19 | SolderMaskMinWidth=0.000000000000 20 | DrawSegmentWidth=0.200000000000 21 | BoardOutlineThickness=0.100000000000 22 | ModuleOutlineThickness=0.150000000000 23 | [cvpcb] 24 | version=1 25 | NetIExt=net 26 | [general] 27 | version=1 28 | [eeschema] 29 | version=1 30 | LibDir= 31 | [eeschema/libraries] 32 | LibName1=power 33 | LibName2=device 34 | LibName3=transistors 35 | LibName4=conn 36 | LibName5=linear 37 | LibName6=regul 38 | LibName7=74xx 39 | LibName8=cmos4000 40 | LibName9=adc-dac 41 | LibName10=memory 42 | LibName11=xilinx 43 | LibName12=microcontrollers 44 | LibName13=dsp 45 | LibName14=microchip 46 | LibName15=analog_switches 47 | LibName16=motorola 48 | LibName17=texas 49 | LibName18=intel 50 | LibName19=audio 51 | LibName20=interface 52 | LibName21=digital-audio 53 | LibName22=philips 54 | LibName23=display 55 | LibName24=cypress 56 | LibName25=siliconi 57 | LibName26=opto 58 | LibName27=atmel 59 | LibName28=contrib 60 | LibName29=valves 61 | LibName30=/home/jvasquez/Projects/double_jump_electric_component_library/double_jump_electric_component_library 62 | LibName31=/home/jvasquez/Projects/synthego_component_library/synthego_component_library 63 | [schematic_editor] 64 | version=1 65 | PageLayoutDescrFile= 66 | PlotDirectoryName= 67 | SubpartIdSeparator=0 68 | SubpartFirstId=65 69 | NetFmtName= 70 | SpiceForceRefPrefix=0 71 | SpiceUseNetNumbers=0 72 | LabSize=60 73 | -------------------------------------------------------------------------------- /pcbs/di2c_breakout/di2c_breakout.sch: -------------------------------------------------------------------------------- 1 | EESchema Schematic File Version 2 2 | LIBS:power 3 | LIBS:device 4 | LIBS:transistors 5 | LIBS:conn 6 | LIBS:linear 7 | LIBS:regul 8 | LIBS:74xx 9 | LIBS:cmos4000 10 | LIBS:adc-dac 11 | LIBS:memory 12 | LIBS:xilinx 13 | LIBS:microcontrollers 14 | LIBS:dsp 15 | LIBS:microchip 16 | LIBS:analog_switches 17 | LIBS:motorola 18 | LIBS:texas 19 | LIBS:intel 20 | LIBS:audio 21 | LIBS:interface 22 | LIBS:digital-audio 23 | LIBS:philips 24 | LIBS:display 25 | LIBS:cypress 26 | LIBS:siliconi 27 | LIBS:opto 28 | LIBS:atmel 29 | LIBS:contrib 30 | LIBS:valves 31 | LIBS:double_jump_electric_component_library 32 | LIBS:synthego_component_library 33 | LIBS:di2c_breakout-cache 34 | EELAYER 25 0 35 | EELAYER END 36 | $Descr A4 11693 8268 37 | encoding utf-8 38 | Sheet 1 1 39 | Title "" 40 | Date "" 41 | Rev "" 42 | Comp "" 43 | Comment1 "" 44 | Comment2 "" 45 | Comment3 "" 46 | Comment4 "" 47 | $EndDescr 48 | NoConn ~ 8200 3300 49 | $Comp 50 | L R R4 51 | U 1 1 587058C5 52 | P 5600 4300 53 | F 0 "R4" V 5680 4300 50 0000 C CNN 54 | F 1 "130" V 5600 4300 50 0000 C CNN 55 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 5530 4300 50 0001 C CNN 56 | F 3 "" H 5600 4300 50 0000 C CNN 57 | F 4 "Panasonic Electronic Components" H 5600 4300 60 0001 C CNN "Manufacturer" 58 | F 5 "ERJ-3EKF1300V" H 5600 4300 60 0001 C CNN "Manufacturer Number" 59 | F 6 "1%" H 5600 4300 60 0001 C CNN "Precision" 60 | 1 5600 4300 61 | 1 0 0 -1 62 | $EndComp 63 | $Comp 64 | L R R5 65 | U 1 1 58705B5F 66 | P 5600 4600 67 | F 0 "R5" V 5680 4600 50 0000 C CNN 68 | F 1 "750" V 5600 4600 50 0000 C CNN 69 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 5530 4600 50 0001 C CNN 70 | F 3 "" H 5600 4600 50 0000 C CNN 71 | F 4 "Panasonic Electronic Components" H 5600 4600 60 0001 C CNN "Manufacturer" 72 | F 5 "ERJ-3EKF7500V" H 5600 4600 60 0001 C CNN "Manufacturer Number" 73 | F 6 "1%" H 5600 4600 60 0001 C CNN "Precision" 74 | 1 5600 4600 75 | 1 0 0 -1 76 | $EndComp 77 | $Comp 78 | L R R3 79 | U 1 1 58705BF4 80 | P 5600 4000 81 | F 0 "R3" V 5680 4000 50 0000 C CNN 82 | F 1 "750" V 5600 4000 50 0000 C CNN 83 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 5530 4000 50 0001 C CNN 84 | F 3 "" H 5600 4000 50 0000 C CNN 85 | F 4 "Panasonic Electronic Components" H 5600 4000 60 0001 C CNN "Manufacturer" 86 | F 5 "ERJ-3EKF7500V" H 5600 4000 60 0001 C CNN "Manufacturer Number" 87 | 1 5600 4000 88 | 1 0 0 -1 89 | $EndComp 90 | $Comp 91 | L R R7 92 | U 1 1 58705E7E 93 | P 6550 3350 94 | F 0 "R7" V 6630 3350 50 0000 C CNN 95 | F 1 "130" V 6550 3350 50 0000 C CNN 96 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 6480 3350 50 0001 C CNN 97 | F 3 "" H 6550 3350 50 0000 C CNN 98 | F 4 "Panasonic Electronic Components" H 6550 3350 60 0001 C CNN "Manufacturer" 99 | F 5 "ERJ-3EKF1300V" H 6550 3350 60 0001 C CNN "Manufacturer Number" 100 | F 6 "1%" H 6550 3350 60 0001 C CNN "Precision" 101 | 1 6550 3350 102 | 1 0 0 -1 103 | $EndComp 104 | $Comp 105 | L R R8 106 | U 1 1 5870605A 107 | P 6550 3650 108 | F 0 "R8" V 6630 3650 50 0000 C CNN 109 | F 1 "750" V 6550 3650 50 0000 C CNN 110 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 6480 3650 50 0001 C CNN 111 | F 3 "" H 6550 3650 50 0000 C CNN 112 | F 4 "Panasonic Electronic Components" H 6550 3650 60 0001 C CNN "Manufacturer" 113 | F 5 "ERJ-3EKF7500V" H 6550 3650 60 0001 C CNN "Manufacturer Number" 114 | F 6 "1%" H 6550 3650 60 0001 C CNN "Precision" 115 | 1 6550 3650 116 | 1 0 0 -1 117 | $EndComp 118 | $Comp 119 | L R R6 120 | U 1 1 587060A2 121 | P 6550 3050 122 | F 0 "R6" V 6630 3050 50 0000 C CNN 123 | F 1 "750" V 6550 3050 50 0000 C CNN 124 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 6480 3050 50 0001 C CNN 125 | F 3 "" H 6550 3050 50 0000 C CNN 126 | F 4 "Panasonic Electronic Components" H 6550 3050 60 0001 C CNN "Manufacturer" 127 | F 5 "ERJ-3EKF7500V" H 6550 3050 60 0001 C CNN "Manufacturer Number" 128 | F 6 "1%" H 6550 3050 60 0001 C CNN "Precision" 129 | 1 6550 3050 130 | 1 0 0 -1 131 | $EndComp 132 | Text Notes 4050 2200 0 60 ~ 0 133 | Populate termination resistors ONLY on the last peripheral in a set. 134 | Text Notes 5400 3350 0 60 ~ 0 135 | DSCL\ntermination\nresistors 136 | Text Notes 6400 2600 0 60 ~ 0 137 | DSDA\ntermination\nresistors 138 | $Comp 139 | L C C2 140 | U 1 1 5879BF70 141 | P 9650 2750 142 | F 0 "C2" H 9675 2850 50 0000 L CNN 143 | F 1 "0.1uF" H 9675 2650 50 0000 L CNN 144 | F 2 "Capacitors_SMD:C_0603_HandSoldering" H 9688 2600 50 0001 C CNN 145 | F 3 "" H 9650 2750 50 0000 C CNN 146 | F 4 "AVX Corporation" H 9650 2750 60 0001 C CNN "Manufacturer" 147 | F 5 "0603YC104KAT2A" H 9650 2750 60 0001 C CNN "Manufacturer Number" 148 | F 6 "10%" H 9650 2750 60 0001 C CNN "Precision" 149 | F 7 "16V" H 9650 2750 60 0001 C CNN "Description" 150 | F 8 "X7R" H 9650 2750 60 0001 C CNN "Temperature Coefficient" 151 | 1 9650 2750 152 | 1 0 0 -1 153 | $EndComp 154 | $Comp 155 | L C C1 156 | U 1 1 5879C083 157 | P 9300 2750 158 | F 0 "C1" H 9325 2850 50 0000 L CNN 159 | F 1 "1uF" H 9325 2650 50 0000 L CNN 160 | F 2 "Capacitors_SMD:C_0603_HandSoldering" H 9338 2600 50 0001 C CNN 161 | F 3 "" H 9300 2750 50 0000 C CNN 162 | F 4 "AVX Corporation" H 9300 2750 60 0001 C CNN "Manufacturer" 163 | F 5 "0603YC105KAT2A" H 9300 2750 60 0001 C CNN "Manufacturer Number" 164 | F 6 "10%" H 9300 2750 60 0001 C CNN "Precision" 165 | F 7 "16V" H 9300 2750 60 0001 C CNN "Description" 166 | F 8 "X7R" H 9300 2750 60 0001 C CNN "Temperature Coefficient" 167 | 1 9300 2750 168 | 1 0 0 -1 169 | $EndComp 170 | Text Label 6350 3300 2 60 ~ 0 171 | DSDA0_N 172 | Text Label 6350 3400 2 60 ~ 0 173 | DSDA0_P 174 | Text Label 5300 4250 2 60 ~ 0 175 | DSCL0_P 176 | Text Label 5300 4350 2 60 ~ 0 177 | DSCL0_N 178 | Text Label 2650 2400 2 60 ~ 0 179 | DSCL0_P 180 | Text Label 2650 2300 2 60 ~ 0 181 | DSCL0_N 182 | Text Label 2650 2600 2 60 ~ 0 183 | DSDA0_N 184 | Text Label 2650 2500 2 60 ~ 0 185 | DSDA0_P 186 | Wire Wire Line 187 | 7000 3550 7100 3550 188 | Wire Wire Line 189 | 7100 3650 7100 3650 190 | Wire Wire Line 191 | 7100 4350 7100 3650 192 | Wire Wire Line 193 | 8500 3450 8200 3450 194 | Wire Wire Line 195 | 8200 3550 8250 3550 196 | Wire Wire Line 197 | 7650 4100 7650 4150 198 | Wire Wire Line 199 | 7000 4250 7000 3550 200 | Wire Wire Line 201 | 7750 2700 7750 2750 202 | Wire Wire Line 203 | 7550 2750 7550 2700 204 | Wire Wire Line 205 | 5450 4150 5750 4150 206 | Wire Wire Line 207 | 5750 4150 5750 4250 208 | Wire Wire Line 209 | 5750 4250 7000 4250 210 | Connection ~ 5600 4150 211 | Wire Wire Line 212 | 5750 4450 5750 4350 213 | Wire Wire Line 214 | 5450 4450 5750 4450 215 | Connection ~ 5600 4450 216 | Wire Wire Line 217 | 5600 4750 5600 4800 218 | Wire Wire Line 219 | 5600 3800 5600 3850 220 | Wire Wire Line 221 | 6350 3300 6450 3300 222 | Wire Wire Line 223 | 6450 3300 6450 3200 224 | Wire Wire Line 225 | 6450 3200 6750 3200 226 | Wire Wire Line 227 | 6750 3200 6750 3300 228 | Wire Wire Line 229 | 6750 3300 7100 3300 230 | Connection ~ 6550 3200 231 | Wire Wire Line 232 | 7100 3400 6750 3400 233 | Wire Wire Line 234 | 6750 3500 6750 3400 235 | Wire Wire Line 236 | 6450 3500 6750 3500 237 | Wire Wire Line 238 | 6450 3500 6450 3400 239 | Wire Wire Line 240 | 6450 3400 6350 3400 241 | Connection ~ 6550 3500 242 | Wire Wire Line 243 | 6550 2700 6550 2900 244 | Wire Notes Line 245 | 6850 4200 6400 4200 246 | Wire Notes Line 247 | 6400 4200 6400 2650 248 | Wire Notes Line 249 | 6400 2650 6850 2650 250 | Wire Notes Line 251 | 6850 2650 6850 4200 252 | Wire Wire Line 253 | 6550 3800 6550 3850 254 | Wire Notes Line 255 | 5800 5000 5800 3400 256 | Wire Notes Line 257 | 5800 3400 5400 3400 258 | Wire Notes Line 259 | 5400 3400 5400 5000 260 | Wire Notes Line 261 | 5400 5000 5800 5000 262 | Wire Wire Line 263 | 9650 2550 9650 2600 264 | Wire Wire Line 265 | 9300 2550 9300 2600 266 | Wire Wire Line 267 | 9300 2950 9300 2900 268 | Wire Wire Line 269 | 9650 2950 9650 2900 270 | Wire Wire Line 271 | 5750 4350 7100 4350 272 | Wire Wire Line 273 | 5450 4150 5450 4250 274 | Wire Wire Line 275 | 5450 4250 5300 4250 276 | Wire Wire Line 277 | 5450 4450 5450 4350 278 | Wire Wire Line 279 | 5450 4350 5300 4350 280 | Wire Wire Line 281 | 3250 2300 3450 2300 282 | Wire Wire Line 283 | 3450 2400 3250 2400 284 | Wire Wire Line 285 | 3450 2500 3250 2500 286 | Wire Wire Line 287 | 3450 2600 3250 2600 288 | Wire Wire Line 289 | 2750 2600 2650 2600 290 | Wire Wire Line 291 | 2750 2500 2650 2500 292 | Wire Wire Line 293 | 2750 2400 2650 2400 294 | Wire Wire Line 295 | 2750 2300 2650 2300 296 | Connection ~ 3450 2600 297 | Connection ~ 3450 2500 298 | Connection ~ 3450 2400 299 | $Comp 300 | L GNDD #PWR01 301 | U 1 1 58797A1E 302 | P 7650 4150 303 | F 0 "#PWR01" H 7650 3900 50 0001 C CNN 304 | F 1 "GNDD" H 7650 4000 50 0000 C CNN 305 | F 2 "" H 7650 4150 50 0000 C CNN 306 | F 3 "" H 7650 4150 50 0000 C CNN 307 | 1 7650 4150 308 | 1 0 0 -1 309 | $EndComp 310 | $Comp 311 | L GNDD #PWR02 312 | U 1 1 58797CB0 313 | P 9650 2950 314 | F 0 "#PWR02" H 9650 2700 50 0001 C CNN 315 | F 1 "GNDD" H 9650 2800 50 0000 C CNN 316 | F 2 "" H 9650 2950 50 0000 C CNN 317 | F 3 "" H 9650 2950 50 0000 C CNN 318 | 1 9650 2950 319 | 1 0 0 -1 320 | $EndComp 321 | $Comp 322 | L GNDD #PWR03 323 | U 1 1 58797E99 324 | P 6700 2750 325 | F 0 "#PWR03" H 6700 2500 50 0001 C CNN 326 | F 1 "GNDD" H 6700 2600 50 0000 C CNN 327 | F 2 "" H 6700 2750 50 0000 C CNN 328 | F 3 "" H 6700 2750 50 0000 C CNN 329 | 1 6700 2750 330 | 1 0 0 -1 331 | $EndComp 332 | $Comp 333 | L GNDD #PWR04 334 | U 1 1 587980B3 335 | P 5600 4800 336 | F 0 "#PWR04" H 5600 4550 50 0001 C CNN 337 | F 1 "GNDD" H 5600 4650 50 0000 C CNN 338 | F 2 "" H 5600 4800 50 0000 C CNN 339 | F 3 "" H 5600 4800 50 0000 C CNN 340 | 1 5600 4800 341 | 1 0 0 -1 342 | $EndComp 343 | $Comp 344 | L GNDD #PWR05 345 | U 1 1 58798216 346 | P 9300 2950 347 | F 0 "#PWR05" H 9300 2700 50 0001 C CNN 348 | F 1 "GNDD" H 9300 2800 50 0000 C CNN 349 | F 2 "" H 9300 2950 50 0000 C CNN 350 | F 3 "" H 9300 2950 50 0000 C CNN 351 | 1 9300 2950 352 | 1 0 0 -1 353 | $EndComp 354 | $Comp 355 | L GNDD #PWR06 356 | U 1 1 587983CF 357 | P 3450 2850 358 | F 0 "#PWR06" H 3450 2600 50 0001 C CNN 359 | F 1 "GNDD" H 3450 2700 50 0000 C CNN 360 | F 2 "" H 3450 2850 50 0000 C CNN 361 | F 3 "" H 3450 2850 50 0000 C CNN 362 | 1 3450 2850 363 | 1 0 0 -1 364 | $EndComp 365 | Wire Wire Line 366 | 3450 2300 3450 2850 367 | $Comp 368 | L CONN_02X05 P1 369 | U 1 1 58780C1B 370 | P 3000 2500 371 | F 0 "P1" H 3000 2800 50 0000 C CNN 372 | F 1 "CONN_02X05" H 3000 2200 50 0000 C CNN 373 | F 2 "double_jump_electric_component_library:3M_ribbon_connector_thru_5x2_.05in_pitch_45210-600230" H 3000 1300 50 0001 C CNN 374 | F 3 "http://multimedia.3m.com/mws/media/905148O/3m-shrouded-boardmount-header-452-series-cd2437.pdf" H 3000 1300 50 0001 C CNN 375 | F 4 "3M" H 3000 2500 60 0001 C CNN "Manufacturer" 376 | F 5 "45210-520230" H 3000 2500 60 0001 C CNN "Manufacturer Number" 377 | 1 3000 2500 378 | 1 0 0 1 379 | $EndComp 380 | Wire Wire Line 381 | 3250 2700 3450 2700 382 | Connection ~ 3450 2700 383 | Wire Wire Line 384 | 2050 2700 2750 2700 385 | $Comp 386 | L PCA9615 U1 387 | U 1 1 588E5A17 388 | P 8000 3900 389 | F 0 "U1" H 8650 4900 60 0000 C CNN 390 | F 1 "PCA9615" H 7950 4950 60 0000 C CNN 391 | F 2 "Housings_SSOP:TSSOP-10_3x3mm_Pitch0.5mm" H 8000 3900 60 0001 C CNN 392 | F 3 "" H 8000 3900 60 0001 C CNN 393 | 1 8000 3900 394 | -1 0 0 -1 395 | $EndComp 396 | $Comp 397 | L CONN_01X04 P2 398 | U 1 1 588E6994 399 | P 3150 3800 400 | F 0 "P2" H 3150 4050 50 0000 C CNN 401 | F 1 "CONN_01X04" V 3250 3800 50 0000 C CNN 402 | F 2 "Pin_Headers:Pin_Header_Straight_1x04_Pitch2.54mm" H 3150 3800 50 0001 C CNN 403 | F 3 "" H 3150 3800 50 0000 C CNN 404 | 1 3150 3800 405 | 1 0 0 -1 406 | $EndComp 407 | $Comp 408 | L CONN_01X04 P3 409 | U 1 1 588E6AB5 410 | P 3150 4450 411 | F 0 "P3" H 3150 4700 50 0000 C CNN 412 | F 1 "CONN_01X04" V 3250 4450 50 0000 C CNN 413 | F 2 "Pin_Headers:Pin_Header_Straight_1x04_Pitch2.54mm" H 3150 4450 50 0001 C CNN 414 | F 3 "" H 3150 4450 50 0000 C CNN 415 | 1 3150 4450 416 | 1 0 0 -1 417 | $EndComp 418 | $Comp 419 | L GNDD #PWR07 420 | U 1 1 588E6B9A 421 | P 2600 3850 422 | F 0 "#PWR07" H 2600 3600 50 0001 C CNN 423 | F 1 "GNDD" H 2600 3700 50 0000 C CNN 424 | F 2 "" H 2600 3850 50 0000 C CNN 425 | F 3 "" H 2600 3850 50 0000 C CNN 426 | 1 2600 3850 427 | 0 1 1 0 428 | $EndComp 429 | Wire Wire Line 430 | 2950 3750 2250 3750 431 | Wire Wire Line 432 | 2950 3650 2250 3650 433 | Text Label 2250 3750 2 60 ~ 0 434 | SDA 435 | Text Label 2250 3650 2 60 ~ 0 436 | SCL 437 | Text Label 8500 3450 0 60 ~ 0 438 | SDA 439 | Text Label 8250 3550 0 60 ~ 0 440 | SCL 441 | Wire Wire Line 442 | 6700 2750 6700 2700 443 | Wire Wire Line 444 | 6700 2700 6550 2700 445 | Wire Wire Line 446 | 2950 3950 2900 3950 447 | Wire Wire Line 448 | 2950 3850 2600 3850 449 | $Comp 450 | L GNDD #PWR08 451 | U 1 1 588ED404 452 | P 2900 4700 453 | F 0 "#PWR08" H 2900 4450 50 0001 C CNN 454 | F 1 "GNDD" H 2900 4550 50 0000 C CNN 455 | F 2 "" H 2900 4700 50 0000 C CNN 456 | F 3 "" H 2900 4700 50 0000 C CNN 457 | 1 2900 4700 458 | 1 0 0 -1 459 | $EndComp 460 | Wire Wire Line 461 | 2900 4400 2900 4700 462 | Wire Wire Line 463 | 2950 4600 2900 4600 464 | Connection ~ 2900 4600 465 | Wire Wire Line 466 | 2950 4500 2900 4500 467 | Connection ~ 2900 4500 468 | $Comp 469 | L R R1 470 | U 1 1 588ED93D 471 | P 1900 2700 472 | F 0 "R1" V 1980 2700 50 0000 C CNN 473 | F 1 "0" V 1900 2700 50 0000 C CNN 474 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 1830 2700 50 0001 C CNN 475 | F 3 "" H 1900 2700 50 0000 C CNN 476 | F 4 "Panasonic Electronic Components" H 1900 2700 60 0001 C CNN "Manufacturer" 477 | F 5 "ERJ-3GEY0R00V" H 1900 2700 60 0001 C CNN "Manufacturer Number" 478 | 1 1900 2700 479 | 0 1 1 0 480 | $EndComp 481 | Wire Wire Line 482 | 1750 2700 1600 2700 483 | $Comp 484 | L R R2 485 | U 1 1 588EDC5C 486 | P 1900 2950 487 | F 0 "R2" V 1980 2950 50 0000 C CNN 488 | F 1 "0" V 1900 2950 50 0000 C CNN 489 | F 2 "Resistors_SMD:R_0603_HandSoldering" V 1830 2950 50 0001 C CNN 490 | F 3 "" H 1900 2950 50 0000 C CNN 491 | F 4 "Panasonic Electronic Components" H 1900 2950 60 0001 C CNN "Manufacturer" 492 | F 5 "ERJ-3GEY0R00V" H 1900 2950 60 0001 C CNN "Manufacturer Number" 493 | 1 1900 2950 494 | 0 1 1 0 495 | $EndComp 496 | $Comp 497 | L GNDD #PWR09 498 | U 1 1 588EDD01 499 | P 1600 2950 500 | F 0 "#PWR09" H 1600 2700 50 0001 C CNN 501 | F 1 "GNDD" H 1600 2800 50 0000 C CNN 502 | F 2 "" H 1600 2950 50 0000 C CNN 503 | F 3 "" H 1600 2950 50 0000 C CNN 504 | 1 1600 2950 505 | 0 1 1 0 506 | $EndComp 507 | Wire Wire Line 508 | 2150 2700 2150 2950 509 | Wire Wire Line 510 | 2150 2950 2050 2950 511 | Connection ~ 2150 2700 512 | Wire Wire Line 513 | 1750 2950 1600 2950 514 | Wire Notes Line 515 | 2200 2600 2200 3100 516 | Wire Notes Line 517 | 2200 3100 1650 3100 518 | Wire Notes Line 519 | 1650 3100 1650 2600 520 | Wire Notes Line 521 | 1650 2600 2200 2600 522 | Text Notes 1650 3300 0 60 ~ 0 523 | Populate one or the other\nbut NOT both 524 | Text GLabel 7550 2700 1 60 Input ~ 0 525 | VDDB 526 | Text GLabel 5600 3800 1 60 Input ~ 0 527 | VDDB 528 | Text GLabel 6550 3850 3 60 Input ~ 0 529 | VDDB 530 | Text GLabel 2700 4300 0 60 Input ~ 0 531 | VDDB 532 | Wire Wire Line 533 | 2950 4400 2900 4400 534 | Wire Wire Line 535 | 2700 4300 2950 4300 536 | Text GLabel 2900 3950 0 60 Input ~ 0 537 | VDDA 538 | Text GLabel 7750 2700 1 60 Input ~ 0 539 | VDDA 540 | Text GLabel 9300 2550 1 60 Input ~ 0 541 | VDDA 542 | Text GLabel 9650 2550 1 60 Input ~ 0 543 | VDDA 544 | $Comp 545 | L C C4 546 | U 1 1 588F0681 547 | P 10600 2750 548 | F 0 "C4" H 10625 2850 50 0000 L CNN 549 | F 1 "0.1uF" H 10625 2650 50 0000 L CNN 550 | F 2 "Capacitors_SMD:C_0603_HandSoldering" H 10638 2600 50 0001 C CNN 551 | F 3 "" H 10600 2750 50 0000 C CNN 552 | F 4 "AVX Corporation" H 10600 2750 60 0001 C CNN "Manufacturer" 553 | F 5 "0603YC104KAT2A" H 10600 2750 60 0001 C CNN "Manufacturer Number" 554 | F 6 "10%" H 10600 2750 60 0001 C CNN "Precision" 555 | F 7 "16V" H 10600 2750 60 0001 C CNN "Description" 556 | F 8 "X7R" H 10600 2750 60 0001 C CNN "Temperature Coefficient" 557 | 1 10600 2750 558 | 1 0 0 -1 559 | $EndComp 560 | $Comp 561 | L C C3 562 | U 1 1 588F068C 563 | P 10250 2750 564 | F 0 "C3" H 10275 2850 50 0000 L CNN 565 | F 1 "1uF" H 10275 2650 50 0000 L CNN 566 | F 2 "Capacitors_SMD:C_0603_HandSoldering" H 10288 2600 50 0001 C CNN 567 | F 3 "" H 10250 2750 50 0000 C CNN 568 | F 4 "AVX Corporation" H 10250 2750 60 0001 C CNN "Manufacturer" 569 | F 5 "0603YC105KAT2A" H 10250 2750 60 0001 C CNN "Manufacturer Number" 570 | F 6 "10%" H 10250 2750 60 0001 C CNN "Precision" 571 | F 7 "16V" H 10250 2750 60 0001 C CNN "Description" 572 | F 8 "X7R" H 10250 2750 60 0001 C CNN "Temperature Coefficient" 573 | 1 10250 2750 574 | 1 0 0 -1 575 | $EndComp 576 | Wire Wire Line 577 | 10600 2550 10600 2600 578 | Wire Wire Line 579 | 10250 2550 10250 2600 580 | Wire Wire Line 581 | 10250 2950 10250 2900 582 | Wire Wire Line 583 | 10600 2950 10600 2900 584 | $Comp 585 | L GNDD #PWR010 586 | U 1 1 588F0696 587 | P 10600 2950 588 | F 0 "#PWR010" H 10600 2700 50 0001 C CNN 589 | F 1 "GNDD" H 10600 2800 50 0000 C CNN 590 | F 2 "" H 10600 2950 50 0000 C CNN 591 | F 3 "" H 10600 2950 50 0000 C CNN 592 | 1 10600 2950 593 | 1 0 0 -1 594 | $EndComp 595 | $Comp 596 | L GNDD #PWR011 597 | U 1 1 588F069C 598 | P 10250 2950 599 | F 0 "#PWR011" H 10250 2700 50 0001 C CNN 600 | F 1 "GNDD" H 10250 2800 50 0000 C CNN 601 | F 2 "" H 10250 2950 50 0000 C CNN 602 | F 3 "" H 10250 2950 50 0000 C CNN 603 | 1 10250 2950 604 | 1 0 0 -1 605 | $EndComp 606 | Text GLabel 10250 2550 1 60 Input ~ 0 607 | VDDB 608 | Text GLabel 10600 2550 1 60 Input ~ 0 609 | VDDB 610 | Text GLabel 1600 2700 0 60 Input ~ 0 611 | VDDA 612 | $EndSCHEMATC 613 | -------------------------------------------------------------------------------- /pcbs/imu_noodle_node_v1/imu_noodle.pro: -------------------------------------------------------------------------------- 1 | update=Mon 20 Nov 2017 06:12:20 PM PST 2 | version=1 3 | last_client=kicad 4 | [pcbnew] 5 | version=1 6 | LastNetListRead= 7 | UseCmpFile=1 8 | PadDrill=0.600000000000 9 | PadDrillOvalY=0.600000000000 10 | PadSizeH=1.500000000000 11 | PadSizeV=1.500000000000 12 | PcbTextSizeV=1.500000000000 13 | PcbTextSizeH=1.500000000000 14 | PcbTextThickness=0.300000000000 15 | ModuleTextSizeV=1.000000000000 16 | ModuleTextSizeH=1.000000000000 17 | ModuleTextSizeThickness=0.150000000000 18 | SolderMaskClearance=0.000000000000 19 | SolderMaskMinWidth=0.000000000000 20 | DrawSegmentWidth=0.200000000000 21 | BoardOutlineThickness=0.100000000000 22 | ModuleOutlineThickness=0.150000000000 23 | [cvpcb] 24 | version=1 25 | NetIExt=net 26 | [general] 27 | version=1 28 | [eeschema] 29 | version=1 30 | LibDir= 31 | [eeschema/libraries] 32 | LibName1=power 33 | LibName2=device 34 | LibName3=transistors 35 | LibName4=conn 36 | LibName5=linear 37 | LibName6=regul 38 | LibName7=74xx 39 | LibName8=cmos4000 40 | LibName9=adc-dac 41 | LibName10=memory 42 | LibName11=xilinx 43 | LibName12=microcontrollers 44 | LibName13=dsp 45 | LibName14=microchip 46 | LibName15=analog_switches 47 | LibName16=motorola 48 | LibName17=texas 49 | LibName18=intel 50 | LibName19=audio 51 | LibName20=interface 52 | LibName21=digital-audio 53 | LibName22=philips 54 | LibName23=display 55 | LibName24=cypress 56 | LibName25=siliconi 57 | LibName26=opto 58 | LibName27=atmel 59 | LibName28=contrib 60 | LibName29=valves 61 | LibName30=/home/jvasquez/Projects/synthego_component_library/synthego_component_library 62 | LibName31=/home/jvasquez/Projects/double_jump_electric_component_library/double_jump_electric_component_library 63 | [schematic_editor] 64 | version=1 65 | PageLayoutDescrFile= 66 | PlotDirectoryName= 67 | SubpartIdSeparator=0 68 | SubpartFirstId=65 69 | NetFmtName= 70 | SpiceForceRefPrefix=0 71 | SpiceUseNetNumbers=0 72 | LabSize=60 73 | -------------------------------------------------------------------------------- /pcbs/imu_noodle_node_v1/imu_noodle_rev-1.1_schematic_output.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Poofjunior/imu_noodle/6a9538ec2ad86af870b3ac77f49af03c439fb0ac/pcbs/imu_noodle_node_v1/imu_noodle_rev-1.1_schematic_output.pdf -------------------------------------------------------------------------------- /pcbs/imu_noodle_node_v2/imu_noodle.pro: -------------------------------------------------------------------------------- 1 | update=Sun 15 Apr 2018 09:24:29 PM PDT 2 | version=1 3 | last_client=kicad 4 | [pcbnew] 5 | version=1 6 | LastNetListRead= 7 | UseCmpFile=1 8 | PadDrill=0.600000000000 9 | PadDrillOvalY=0.600000000000 10 | PadSizeH=1.500000000000 11 | PadSizeV=1.500000000000 12 | PcbTextSizeV=1.500000000000 13 | PcbTextSizeH=1.500000000000 14 | PcbTextThickness=0.300000000000 15 | ModuleTextSizeV=1.000000000000 16 | ModuleTextSizeH=1.000000000000 17 | ModuleTextSizeThickness=0.150000000000 18 | SolderMaskClearance=0.000000000000 19 | SolderMaskMinWidth=0.000000000000 20 | DrawSegmentWidth=0.200000000000 21 | BoardOutlineThickness=0.100000000000 22 | ModuleOutlineThickness=0.150000000000 23 | [cvpcb] 24 | version=1 25 | NetIExt=net 26 | [general] 27 | version=1 28 | [eeschema] 29 | version=1 30 | LibDir= 31 | [eeschema/libraries] 32 | LibName1=power 33 | LibName2=device 34 | LibName3=transistors 35 | LibName4=conn 36 | LibName5=linear 37 | LibName6=regul 38 | LibName7=74xx 39 | LibName8=cmos4000 40 | LibName9=adc-dac 41 | LibName10=memory 42 | LibName11=xilinx 43 | LibName12=microcontrollers 44 | LibName13=dsp 45 | LibName14=microchip 46 | LibName15=analog_switches 47 | LibName16=motorola 48 | LibName17=texas 49 | LibName18=intel 50 | LibName19=audio 51 | LibName20=interface 52 | LibName21=digital-audio 53 | LibName22=philips 54 | LibName23=display 55 | LibName24=cypress 56 | LibName25=siliconi 57 | LibName26=opto 58 | LibName27=atmel 59 | LibName28=contrib 60 | LibName29=valves 61 | LibName30=/home/jvasquez/Projects/synthego_component_library/synthego_component_library 62 | LibName31=/home/jvasquez/Projects/double_jump_electric_component_library/double_jump_electric_component_library 63 | [schematic_editor] 64 | version=1 65 | PageLayoutDescrFile= 66 | PlotDirectoryName= 67 | SubpartIdSeparator=0 68 | SubpartFirstId=65 69 | NetFmtName= 70 | SpiceForceRefPrefix=0 71 | SpiceUseNetNumbers=0 72 | LabSize=60 73 | -------------------------------------------------------------------------------- /pcbs/imu_noodle_node_v2/imu_noodle_rev-2.0_schematic_output.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Poofjunior/imu_noodle/6a9538ec2ad86af870b3ac77f49af03c439fb0ac/pcbs/imu_noodle_node_v2/imu_noodle_rev-2.0_schematic_output.pdf -------------------------------------------------------------------------------- /pcbs/notes/ltc4316_address_translator.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 13, 6 | "metadata": { 7 | "collapsed": false 8 | }, 9 | "outputs": [], 10 | "source": [ 11 | "import math\n", 12 | "\n", 13 | "class ResistorPicker(object):\n", 14 | " \"\"\"Picks 1% resistor values for a desired i2c address given a minimum resistance constraint.\n", 15 | " \n", 16 | " Breakdown of Constraints satisfied:\n", 17 | " -- resistors have a minimum resistance\n", 18 | " -- resistors fall within +/-0.015 of the setpoint XORL or XORH ratios\n", 19 | " -- resistors real 1% resistor values\n", 20 | " \"\"\"\n", 21 | " X_RATIO_TOLERANCE = 0.015\n", 22 | " OHM_STEP = 1000\n", 23 | " \n", 24 | " bits_to_xorl_over_vcc = {0b0000: 0.0,\n", 25 | " 0b0001: .09375,\n", 26 | " 0b0010: .15625,\n", 27 | " 0b0011: .21875,\n", 28 | " 0b0100: .28125,\n", 29 | " 0b0101: .34375,\n", 30 | " 0b0110: .40625,\n", 31 | " 0b0111: .46875,\n", 32 | " 0b1000: .53125,\n", 33 | " 0b1001: .59375,\n", 34 | " 0b1010: .65625,\n", 35 | " 0b1011: .71875,\n", 36 | " 0b1100: .78125,\n", 37 | " 0b1101: .84375,\n", 38 | " 0b1110: .90625,\n", 39 | " 0b1111: .96875}\n", 40 | " bits_to_xorh_over_vcc = {0b000: 0.0,\n", 41 | " 0b001: .09375,\n", 42 | " 0b010: .15625,\n", 43 | " 0b011: .21875,\n", 44 | " 0b100: .28125,\n", 45 | " 0b101: .34375,\n", 46 | " 0b110: .40625,\n", 47 | " 0b111: .46875}\n", 48 | " \n", 49 | " # significant-figure equation from:\n", 50 | " # https://stackoverflow.com/questions/3410976/how-to-round-a-number-to-significant-figures-in-python?utm_medium=organic&utm_source=google_rich_qa&utm_campaign=google_rich_qa \n", 51 | " R_E96_10K = [10**(i/96) * 10000 for i in range(0, 96)]\n", 52 | " R_E96_10K = [round( x, int( 3 - math.ceil(math.log10(x)) ) ) for x in R_E96_10K]\n", 53 | " \n", 54 | " R_E96_1K = [10**(i/96) * 1000 for i in range(0, 96)]\n", 55 | " R_E96_1K = [round( x, int( 3 - math.ceil(math.log10(x)) ) ) for x in R_E96_1K]\n", 56 | " \n", 57 | " def __init__(self, min_total_r, default_device_address):\n", 58 | " self.r_total = min_total_r\n", 59 | " self.default_device_address = default_device_address\n", 60 | " return\n", 61 | " \n", 62 | " def compute_resistor_values(self, desired_address):\n", 63 | " #print(\"Desired Address: {}\".format(hex(desired_address)))\n", 64 | " des_xorl_ratio, des_xorh_ratio = self.get_xorl_and_xorh_ratios(desired_address)\n", 65 | " #print(\"desired xorl_ratio: {} | desired xorh_ratio: {}\".format(des_xorl_ratio, des_xorh_ratio))\n", 66 | " ra1, ra2, ra3 = self.get_resistor_triad(des_xorl_ratio, des_xorh_ratio)\n", 67 | " actual_xorl_ratio, actual_xorh_ratio = self._get_x_ratios_from_resistors(ra1, ra2, ra3)\n", 68 | " \n", 69 | " #print(\"actual xorl_ratio: {:.5f} | actual xorh_ratio: {:.5f}\".format(actual_xorl_ratio, actual_xorh_ratio))\n", 70 | " if not (self.x_ratio_in_bounds(des_xorl_ratio, actual_xorl_ratio) and \\\n", 71 | " self.x_ratio_in_bounds(des_xorh_ratio, actual_xorh_ratio)):\n", 72 | " old_r_total = self.r_total\n", 73 | " self.r_total += ResistorPicker.OHM_STEP\n", 74 | " print(\"failed to find real r-triad {} for address {} and min total r: {}\"\n", 75 | " .format((ra1, ra2, ra3), hex(desired_address), self.r_total))\n", 76 | " return self.compute_resistor_values(desired_address)\n", 77 | " self.r_total = old_r_total\n", 78 | " return\n", 79 | " return (ra1, ra2, ra3)\n", 80 | " \n", 81 | " def get_resistor_triad(self, xorl_over_vcc, xorh_over_vcc):\n", 82 | " r_a3 = self.r_total * xorh_over_vcc\n", 83 | " r_a2 = self.r_total * xorl_over_vcc - r_a3\n", 84 | " r_a1 = self.r_total - r_a3 - r_a2\n", 85 | " #print(\"{} | {} | {}\".format(r_a1, r_a2, r_a3))\n", 86 | " if r_a3 <= 9760:\n", 87 | " r_a3 = ResistorPicker._r_to_e961k(r_a3)\n", 88 | " else:\n", 89 | " r_a3 = ResistorPicker._r_to_e9610k(r_a3)\n", 90 | " if r_a2 <= 9760:\n", 91 | " r_a2 = ResistorPicker._r_to_e961k(r_a2)\n", 92 | " else:\n", 93 | " r_a2 = ResistorPicker._r_to_e9610k(r_a2)\n", 94 | " if r_a1 <= 9760:\n", 95 | " r_a1 = ResistorPicker._r_to_e961k(r_a1)\n", 96 | " else:\n", 97 | " r_a1 = ResistorPicker._r_to_e9610k(r_a1)\n", 98 | "\n", 99 | " return (r_a1, r_a2, r_a3)\n", 100 | " \n", 101 | " @classmethod\n", 102 | " def _r_to_e9610k(cls, resistor_value):\n", 103 | " \"\"\"Compute the closest 10K-100K 1% resistor value for the given resistance value.\n", 104 | " \"\"\"\n", 105 | " if resistor_value == 0:\n", 106 | " return 0\n", 107 | " index = cls._r_to_e9610k_index(resistor_value)\n", 108 | " return cls.R_E96_10K[index]\n", 109 | " \n", 110 | " @staticmethod\n", 111 | " def _r_to_e9610k_index(resistor_value):\n", 112 | " \"\"\"Compute the index to the closest 10K-100K 1% resistor value for the given resistance value.\n", 113 | " This is just the inverse function for generating the E96 resistor values.\n", 114 | " \"\"\"\n", 115 | " index = round(math.log((resistor_value/10000)**(96/math.log(10))))\n", 116 | " if 0 > index > 95:\n", 117 | " raise IndexError(\"{} out of bounds.\".format(index))\n", 118 | " return index\n", 119 | " \n", 120 | " @classmethod\n", 121 | " def _r_to_e961k(cls, resistor_value):\n", 122 | " \"\"\"Compute the closest 1K-10K 1% resistor value for the given resistance value.\n", 123 | " \"\"\"\n", 124 | " if resistor_value <= 10:\n", 125 | " return 0\n", 126 | " index = cls._r_to_e961k_index(resistor_value)\n", 127 | " return cls.R_E96_1K[index]\n", 128 | " \n", 129 | " @staticmethod\n", 130 | " def _r_to_e961k_index(resistor_value):\n", 131 | " \"\"\"Compute the index to the closest 1K-10K 1% resistor value for the given resistance value.\n", 132 | " This is just the inverse function for generating the E96 resistor values.\n", 133 | " \"\"\"\n", 134 | " index = round(math.log((resistor_value/1000)**(96/math.log(10))))\n", 135 | " if 0 > index > 95:\n", 136 | " raise IndexError(\"{} out of bounds.\".format(index))\n", 137 | " return index\n", 138 | " \n", 139 | " def _get_translation(self, desired_address):\n", 140 | " translation = self.default_device_address ^ desired_address\n", 141 | " return translation\n", 142 | " \n", 143 | " def get_xorl_and_xorh_ratios(self, desired_address):\n", 144 | " translation_byte = self._get_translation(desired_address)\n", 145 | " upper_bits = (translation_byte & 0x70) >> 4\n", 146 | " lower_bits = (translation_byte & 0x0F)\n", 147 | " xorl_ratio = ResistorPicker.bits_to_xorl_over_vcc[lower_bits]\n", 148 | " xorh_ratio = ResistorPicker.bits_to_xorh_over_vcc[upper_bits]\n", 149 | " return [xorl_ratio, xorh_ratio]\n", 150 | " \n", 151 | " def _get_x_ratios_from_resistors(self, ra1, ra2, ra3):\n", 152 | " r_total = ra1 + ra2 + ra3\n", 153 | " xorh_ratio = ra3/r_total\n", 154 | " xorl_ratio = (ra2 + ra3)/r_total\n", 155 | " return [xorl_ratio, xorh_ratio]\n", 156 | " \n", 157 | " @classmethod\n", 158 | " def x_ratio_in_bounds(cls, desired_x_ratio, actual_x_ratio):\n", 159 | " return abs(desired_x_ratio - actual_x_ratio) <= cls.X_RATIO_TOLERANCE" 160 | ] 161 | }, 162 | { 163 | "cell_type": "code", 164 | "execution_count": 18, 165 | "metadata": { 166 | "collapsed": false 167 | }, 168 | "outputs": [ 169 | { 170 | "name": "stdout", 171 | "output_type": "stream", 172 | "text": [ 173 | "{'0x20': (42200.0, 47500.0, 0),\n", 174 | " '0x21': (36500.0, 53600.0, 0),\n", 175 | " '0x22': (30900.0, 59000.0, 0),\n", 176 | " '0x23': (25500.0, 64900.0, 0),\n", 177 | " '0x24': (19600.0, 69800.0, 0),\n", 178 | " '0x25': (14000.0, 76800.0, 0),\n", 179 | " '0x26': (8450.0, 82500.0, 0),\n", 180 | " '0x27': (2800.0, 86600.0, 0),\n", 181 | " '0x28': (90900.0, 0, 0),\n", 182 | " '0x29': (82500.0, 8450.0, 0),\n", 183 | " '0x2a': (76800.0, 14000.0, 0),\n", 184 | " '0x2b': (69800.0, 19600.0, 0),\n", 185 | " '0x2c': (64900.0, 25500.0, 0),\n", 186 | " '0x2d': (59000.0, 30900.0, 0),\n", 187 | " '0x2e': (53600.0, 36500.0, 0),\n", 188 | " '0x2f': (47500.0, 42200.0, 0),\n", 189 | " '0x30': (42200.0, 39200.0, 8450.0),\n", 190 | " '0x31': (36500.0, 45300.0, 8450.0),\n", 191 | " '0x32': (30900.0, 51100.0, 8450.0),\n", 192 | " '0x33': (25500.0, 56200.0, 8450.0),\n", 193 | " '0x34': (19600.0, 61900.0, 8450.0),\n", 194 | " '0x35': (14000.0, 68100.0, 8450.0),\n", 195 | " '0x36': (8450.0, 73200.0, 8450.0),\n", 196 | " '0x37': (2800.0, 78700.0, 8450.0)}\n" 197 | ] 198 | } 199 | ], 200 | "source": [ 201 | "address_range = range(0x20, 0x38)\n", 202 | "picker = ResistorPicker(90000, 0x28)\n", 203 | "address_booklet = {}\n", 204 | "for address in address_range:\n", 205 | " ra123 = picker.compute_resistor_values(address)\n", 206 | " #print(\"ra1 = {} | ra2 = {} | ra3 = {}\".format(ra1, ra2, ra3))\n", 207 | " address_booklet[hex(address)] = ra123\n", 208 | "import pprint\n", 209 | "pprint.pprint(address_booklet)" 210 | ] 211 | }, 212 | { 213 | "cell_type": "code", 214 | "execution_count": null, 215 | "metadata": { 216 | "collapsed": true 217 | }, 218 | "outputs": [], 219 | "source": [] 220 | } 221 | ], 222 | "metadata": { 223 | "anaconda-cloud": {}, 224 | "kernelspec": { 225 | "display_name": "Python 3", 226 | "language": "python", 227 | "name": "python3" 228 | }, 229 | "language_info": { 230 | "codemirror_mode": { 231 | "name": "ipython", 232 | "version": 3 233 | }, 234 | "file_extension": ".py", 235 | "mimetype": "text/x-python", 236 | "name": "python", 237 | "nbconvert_exporter": "python", 238 | "pygments_lexer": "ipython3", 239 | "version": "3.5.2" 240 | } 241 | }, 242 | "nbformat": 4, 243 | "nbformat_minor": 0 244 | } 245 | -------------------------------------------------------------------------------- /visualization/line_viz/Makefile: -------------------------------------------------------------------------------- 1 | # Attempt to load a config.make file. 2 | # If none is found, project defaults in config.project.make will be used. 3 | ifneq ($(wildcard config.make),) 4 | include config.make 5 | endif 6 | 7 | # make sure the the OF_ROOT location is defined 8 | ifndef OF_ROOT 9 | OF_ROOT=$(realpath ../../..) 10 | endif 11 | 12 | # call the project makefile! 13 | include $(OF_ROOT)/libs/openFrameworksCompiled/project/makefileCommon/compile.project.mk 14 | -------------------------------------------------------------------------------- /visualization/line_viz/config.make: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # CONFIGURE PROJECT MAKEFILE (optional) 3 | # This file is where we make project specific configurations. 4 | ################################################################################ 5 | 6 | ################################################################################ 7 | # OF ROOT 8 | # The location of your root openFrameworks installation 9 | # (default) OF_ROOT = ../../.. 10 | ################################################################################ 11 | # OF_ROOT = ../../.. 12 | 13 | ################################################################################ 14 | # PROJECT ROOT 15 | # The location of the project - a starting place for searching for files 16 | # (default) PROJECT_ROOT = . (this directory) 17 | # 18 | ################################################################################ 19 | # PROJECT_ROOT = . 20 | 21 | ################################################################################ 22 | # PROJECT SPECIFIC CHECKS 23 | # This is a project defined section to create internal makefile flags to 24 | # conditionally enable or disable the addition of various features within 25 | # this makefile. For instance, if you want to make changes based on whether 26 | # GTK is installed, one might test that here and create a variable to check. 27 | ################################################################################ 28 | # None 29 | 30 | ################################################################################ 31 | # PROJECT EXTERNAL SOURCE PATHS 32 | # These are fully qualified paths that are not within the PROJECT_ROOT folder. 33 | # Like source folders in the PROJECT_ROOT, these paths are subject to 34 | # exclusion via the PROJECT_EXLCUSIONS list. 35 | # 36 | # (default) PROJECT_EXTERNAL_SOURCE_PATHS = (blank) 37 | # 38 | # Note: Leave a leading space when adding list items with the += operator 39 | ################################################################################ 40 | PROJECT_EXTERNAL_SOURCE_PATHS = ../../common/usb_packet 41 | 42 | ################################################################################ 43 | # PROJECT EXCLUSIONS 44 | # These makefiles assume that all folders in your current project directory 45 | # and any listed in the PROJECT_EXTERNAL_SOURCH_PATHS are are valid locations 46 | # to look for source code. The any folders or files that match any of the 47 | # items in the PROJECT_EXCLUSIONS list below will be ignored. 48 | # 49 | # Each item in the PROJECT_EXCLUSIONS list will be treated as a complete 50 | # string unless teh user adds a wildcard (%) operator to match subdirectories. 51 | # GNU make only allows one wildcard for matching. The second wildcard (%) is 52 | # treated literally. 53 | # 54 | # (default) PROJECT_EXCLUSIONS = (blank) 55 | # 56 | # Will automatically exclude the following: 57 | # 58 | # $(PROJECT_ROOT)/bin% 59 | # $(PROJECT_ROOT)/obj% 60 | # $(PROJECT_ROOT)/%.xcodeproj 61 | # 62 | # Note: Leave a leading space when adding list items with the += operator 63 | ################################################################################ 64 | # PROJECT_EXCLUSIONS = 65 | 66 | ################################################################################ 67 | # PROJECT LINKER FLAGS 68 | # These flags will be sent to the linker when compiling the executable. 69 | # 70 | # (default) PROJECT_LDFLAGS = -Wl,-rpath=./libs 71 | # 72 | # Note: Leave a leading space when adding list items with the += operator 73 | # 74 | # Currently, shared libraries that are needed are copied to the 75 | # $(PROJECT_ROOT)/bin/libs directory. The following LDFLAGS tell the linker to 76 | # add a runtime path to search for those shared libraries, since they aren't 77 | # incorporated directly into the final executable application binary. 78 | ################################################################################ 79 | # PROJECT_LDFLAGS=-Wl,-rpath=./libs 80 | # -lusb-1.0 derived from running pkg-config --libs libusb-1.0 81 | PROJECT_LDFLAGS=-lusb-1.0 82 | ################################################################################ 83 | # PROJECT DEFINES 84 | # Create a space-delimited list of DEFINES. The list will be converted into 85 | # CFLAGS with the "-D" flag later in the makefile. 86 | # 87 | # (default) PROJECT_DEFINES = (blank) 88 | # 89 | # Note: Leave a leading space when adding list items with the += operator 90 | ################################################################################ 91 | # PROJECT_DEFINES = 92 | 93 | ################################################################################ 94 | # PROJECT CFLAGS 95 | # This is a list of fully qualified CFLAGS required when compiling for this 96 | # project. These CFLAGS will be used IN ADDITION TO the PLATFORM_CFLAGS 97 | # defined in your platform specific core configuration files. These flags are 98 | # presented to the compiler BEFORE the PROJECT_OPTIMIZATION_CFLAGS below. 99 | # 100 | # (default) PROJECT_CFLAGS = (blank) 101 | # 102 | # Note: Before adding PROJECT_CFLAGS, note that the PLATFORM_CFLAGS defined in 103 | # your platform specific configuration file will be applied by default and 104 | # further flags here may not be needed. 105 | # 106 | # Note: Leave a leading space when adding list items with the += operator 107 | ################################################################################ 108 | #PROJECT_CFLAGS = 109 | 110 | ################################################################################ 111 | # PROJECT OPTIMIZATION CFLAGS 112 | # These are lists of CFLAGS that are target-specific. While any flags could 113 | # be conditionally added, they are usually limited to optimization flags. 114 | # These flags are added BEFORE the PROJECT_CFLAGS. 115 | # 116 | # PROJECT_OPTIMIZATION_CFLAGS_RELEASE flags are only applied to RELEASE targets. 117 | # 118 | # (default) PROJECT_OPTIMIZATION_CFLAGS_RELEASE = (blank) 119 | # 120 | # PROJECT_OPTIMIZATION_CFLAGS_DEBUG flags are only applied to DEBUG targets. 121 | # 122 | # (default) PROJECT_OPTIMIZATION_CFLAGS_DEBUG = (blank) 123 | # 124 | # Note: Before adding PROJECT_OPTIMIZATION_CFLAGS, please note that the 125 | # PLATFORM_OPTIMIZATION_CFLAGS defined in your platform specific configuration 126 | # file will be applied by default and further optimization flags here may not 127 | # be needed. 128 | # 129 | # Note: Leave a leading space when adding list items with the += operator 130 | ################################################################################ 131 | # PROJECT_OPTIMIZATION_CFLAGS_RELEASE = 132 | # PROJECT_OPTIMIZATION_CFLAGS_DEBUG = 133 | 134 | ################################################################################ 135 | # PROJECT COMPILERS 136 | # Custom compilers can be set for CC and CXX 137 | # (default) PROJECT_CXX = (blank) 138 | # (default) PROJECT_CC = (blank) 139 | # Note: Leave a leading space when adding list items with the += operator 140 | ################################################################################ 141 | # PROJECT_CXX = 142 | # PROJECT_CC = 143 | -------------------------------------------------------------------------------- /visualization/line_viz/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "ofMain.h" 2 | #include "ofApp.h" 3 | 4 | //======================================================================== 5 | int main( ){ 6 | 7 | ofSetupOpenGL(1024,768, OF_WINDOW); // <-------- setup the GL context 8 | 9 | // this kicks off the running of my app 10 | // can be OF_WINDOW or OF_FULLSCREEN 11 | // pass in width and height too: 12 | ofRunApp( new ofApp()); 13 | 14 | } 15 | -------------------------------------------------------------------------------- /visualization/line_viz/src/node_usb_driver.cpp: -------------------------------------------------------------------------------- 1 | #include "node_usb_driver.h" 2 | // http://www.dreamincode.net/forums/topic/148707-introduction-to-using-libusb-10/ 3 | NodeUsbDriver::NodeUsbDriver() 4 | { 5 | libusb_device **devs; //pointer to pointer of device, used to retrieve a list of devices 6 | libusb_context *ctx_ = NULL; //a libusb session 7 | ssize_t cnt; // number of devices in list 8 | // Initialize the library for the session we just declared 9 | if(libusb_init(&ctx_) < 0) { 10 | std::cout << "Init Error "<< std::endl; //there was an error 11 | } 12 | libusb_set_debug(ctx_, 3); //set verbosity level to 3, as suggested in the documentation 13 | 14 | cnt = libusb_get_device_list(ctx_, &devs); //get the list of devices 15 | if(cnt < 0) { 16 | std::cout<<"Get Device Error"< 5 | #include 6 | #include 7 | #include 8 | 9 | class NodeUsbDriver 10 | { 11 | public: 12 | 13 | NodeUsbDriver(); 14 | 15 | ~NodeUsbDriver(); 16 | 17 | void updateNodes(); 18 | 19 | libusb_device_handle* devHandle_; 20 | libusb_context* ctx_; 21 | 22 | static const size_t NUM_NODES = 5; // total number of nodes 23 | static const size_t NUM_BYTES = 64; // teensy bulk transfer size. 24 | 25 | /// The angles of each imu node. 26 | Quaternion orientations_[NUM_NODES]; 27 | 28 | private: 29 | static const uint16_t TEENSY_VID = 0x16C0; 30 | static const uint16_t TEENSY_PID = 0x0486; 31 | 32 | USBPacket usbPacket_; 33 | }; 34 | #endif //NODE_USB_H 35 | 36 | -------------------------------------------------------------------------------- /visualization/line_viz/src/ofApp.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | */ 4 | 5 | #include "ofApp.h" 6 | 7 | //-------------------------------------------------------------- 8 | void ofApp::setup(){ 9 | ofNoFill(); 10 | ofSetFrameRate(60); 11 | ofBackground(0); 12 | 13 | mesh_.setMode(OF_PRIMITIVE_LINE_LOOP); 14 | 15 | size_t numVerts = 24; 16 | float w = 6; 17 | float h = 6; 18 | for (unsigned i = 0; i < numVerts; ++i) 19 | { 20 | mesh_.addVertex(ofVec3f(0.f, 21 | w * cos(TWO_PI * i / (float)numVerts), 22 | h * sin(TWO_PI * i / (float)numVerts))); 23 | } 24 | } 25 | 26 | //-------------------------------------------------------------- 27 | void ofApp::update(){ 28 | nodeUsbDriver_.updateNodes(); 29 | } 30 | 31 | //-------------------------------------------------------------- 32 | void ofApp::draw(){ 33 | 34 | //ofPolyline line; 35 | ofxPtf ptf; 36 | cam_.begin(); 37 | cam_.setVFlip(true); 38 | 39 | 40 | ofSetLineWidth(10); 41 | 42 | ofPushMatrix(); 43 | //ofDrawGrid( 44 | 45 | Quaternion slerpQuat; 46 | float percentRotation; 47 | ofVec3f lastNoodleVertex{0, 0, 0}; 48 | ofVec3f noodleVertex{1, 0, 0}; 49 | 50 | // Start the line at the origin. 51 | //line.addVertex(lastNoodleVertex); 52 | ptf.addPoint(lastNoodleVertex); 53 | for (size_t nodeIndex = 0; nodeIndex < NodeUsbDriver::NUM_NODES - 1; ++nodeIndex) 54 | { 55 | for (size_t segmentIndex = 0; segmentIndex < SUBDIVISIONS; ++segmentIndex) 56 | { 57 | /// Reset the current vector segment to the default orientation. 58 | noodleVertex.x = 1.0; 59 | noodleVertex.y = 0.0; 60 | noodleVertex.z = 0.0; 61 | 62 | /// Compute the next quaternion in the slerp iteration. 63 | percentRotation = float(segmentIndex+1)/SUBDIVISIONS; 64 | slerpQuat = Quaternion::slerp(nodeUsbDriver_.orientations_[nodeIndex], 65 | nodeUsbDriver_.orientations_[nodeIndex + 1], 66 | percentRotation); 67 | 68 | /// Rotate the current vector by the slerp quaternion amount. 69 | slerpQuat.rotate(noodleVertex.x, 70 | noodleVertex.y, 71 | noodleVertex.z); 72 | /// Scale the current vector to the proper segment length 73 | noodleVertex *= SEGMENT_LENGTH_PX; 74 | 75 | /// Translate to the tip of the previous segment with good ol' vector addition 76 | /// (Exclude the first segment). 77 | noodleVertex += lastNoodleVertex; 78 | 79 | /// Add it to the current line. 80 | //line.addVertex(noodleVertex); 81 | ptf.addPoint(noodleVertex); 82 | lastNoodleVertex = noodleVertex; 83 | } 84 | } 85 | 86 | //line.draw(); 87 | 88 | for (int i = 0; i < ptf.framesSize(); ++i) 89 | { 90 | ofPushMatrix(); 91 | 92 | // multiply current matrix (rotated around x axis) 93 | // by transform for next frame 94 | ofMultMatrix(ptf.frameAt(i)); 95 | 96 | // draw ellipse 97 | mesh_.draw(); 98 | 99 | ofPopMatrix(); 100 | } 101 | 102 | 103 | ofPopMatrix(); 104 | cam_.end(); 105 | } 106 | 107 | //-------------------------------------------------------------- 108 | void ofApp::keyPressed(int key){ 109 | } 110 | 111 | //-------------------------------------------------------------- 112 | void ofApp::keyReleased(int key){ 113 | 114 | } 115 | 116 | //-------------------------------------------------------------- 117 | void ofApp::mouseMoved(int x, int y ){ 118 | 119 | } 120 | 121 | //-------------------------------------------------------------- 122 | void ofApp::mouseDragged(int x, int y, int button){ 123 | 124 | } 125 | 126 | //-------------------------------------------------------------- 127 | void ofApp::mousePressed(int x, int y, int button){ 128 | //store the last mouse point when it's first pressed to prevent popping 129 | } 130 | 131 | //-------------------------------------------------------------- 132 | void ofApp::mouseReleased(int x, int y, int button){ 133 | 134 | } 135 | 136 | //-------------------------------------------------------------- 137 | void ofApp::mouseEntered(int x, int y){ 138 | 139 | } 140 | 141 | //-------------------------------------------------------------- 142 | void ofApp::mouseExited(int x, int y){ 143 | 144 | } 145 | 146 | //-------------------------------------------------------------- 147 | void ofApp::windowResized(int w, int h){ 148 | 149 | } 150 | 151 | //-------------------------------------------------------------- 152 | void ofApp::gotMessage(ofMessage msg){ 153 | 154 | } 155 | 156 | //-------------------------------------------------------------- 157 | void ofApp::dragEvent(ofDragInfo dragInfo){ 158 | 159 | } 160 | -------------------------------------------------------------------------------- /visualization/line_viz/src/ofApp.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Joshua Vasquez 4 | * 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "ofMain.h" 10 | #include "qpose/src/quaternion.hpp" 11 | #include "node_usb_driver.h" 12 | #include "ofxptf/ofxPtf.h" 13 | 14 | class ofApp : public ofBaseApp{ 15 | 16 | public: 17 | void setup(); 18 | void update(); 19 | void draw(); 20 | 21 | void keyPressed (int key); 22 | void keyReleased(int key); 23 | void mouseMoved(int x, int y ); 24 | void mouseDragged(int x, int y, int button); 25 | void mousePressed(int x, int y, int button); 26 | void mouseReleased(int x, int y, int button); 27 | void mouseEntered(int x, int y); 28 | void mouseExited(int x, int y); 29 | void windowResized(int w, int h); 30 | void dragEvent(ofDragInfo dragInfo); 31 | void gotMessage(ofMessage msg); 32 | 33 | static const size_t SUBDIVISIONS = 8; // line segments between two adjacent nodes. 34 | static const size_t NOODLE_LENGTH_PX = 400; // total length of the noodle line (in pixels) 35 | static const size_t SEGMENT_LENGTH_PX = NOODLE_LENGTH_PX/(SUBDIVISIONS * NodeUsbDriver::NUM_NODES); 36 | 37 | /// The angles of each imu node. 38 | //Quaternion orientations_[NodeUsbDriver::NUM_NODES]; 39 | // in the NodeUsbDriver now. 40 | 41 | NodeUsbDriver nodeUsbDriver_; 42 | 43 | ofEasyCam cam_; 44 | ofVboMesh mesh_; 45 | //ofxPtf ptf_; 46 | }; 47 | -------------------------------------------------------------------------------- /visualization/line_viz/src/ofxptf/ParallelTransportFrames.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ParallelTransportFrames.cpp 3 | * 4 | * Copyright (c) 2012, Neil Mendoza, http://www.neilmendoza.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of Neil Mendoza nor the names of its contributors may be used 16 | * to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | */ 32 | #include "ParallelTransportFrames.h" 33 | 34 | namespace itg 35 | { 36 | ParallelTransportFrames::ParallelTransportFrames() : 37 | maxPoints(4), maxFrames(numeric_limits::max()) 38 | { 39 | 40 | } 41 | 42 | bool ParallelTransportFrames::addPoint(const ofVec3f& point) 43 | { 44 | points.push_back(point); 45 | while (points.size() > maxPoints) points.pop_front(); 46 | if (points.size() == 3) firstFrame(); 47 | else if (points.size() > 3) 48 | { 49 | nextFrame(); 50 | return true; 51 | } 52 | return false; 53 | } 54 | 55 | void ParallelTransportFrames::firstFrame() 56 | { 57 | ofVec3f t = ( points[1] - points[0] ).normalize(); 58 | 59 | ofVec3f n = t.crossed( points[2] - points[0] ).normalize(); 60 | if( n.length() == 0.0f ) 61 | { 62 | int i = fabs( t[0] ) < fabs( t[1] ) ? 0 : 1; 63 | if( fabs( t[2] ) < fabs( t[i] ) ) i = 2; 64 | 65 | ofVec3f v; 66 | v[i] = 1.f; 67 | n = t.crossed( v ).normalize(); 68 | } 69 | 70 | ofVec3f b = t.crossed( n ); 71 | 72 | ofMatrix4x4 m(t[0], t[1], t[2], 0.0, 73 | b[0], b[1], b[2], 0.0, 74 | n[0], n[1], n[2], 0.0, 75 | points[0][0], points[0][1], points[0][2], 1.f); 76 | 77 | frames.push_back(m); 78 | 79 | prevTangent = t; 80 | startNormal = n; 81 | } 82 | 83 | void ParallelTransportFrames::nextFrame() 84 | { 85 | curTangent = points.back() - points[points.size() - 2]; 86 | ofVec3f a; // Rotation axis. 87 | float r = 0; // Rotation angle. 88 | 89 | if( ( prevTangent.lengthSquared() != 0.0 ) && ( curTangent.lengthSquared() != 0.0 ) ) 90 | { 91 | curTangent.normalize(); 92 | float dot = prevTangent.dot( curTangent ); 93 | 94 | if( dot > 1.f ) dot = 1.f; 95 | else if( dot < -1.0 ) dot = -1.0; 96 | 97 | r = acos( dot ); 98 | a = prevTangent.crossed( curTangent ); 99 | } 100 | 101 | if( ( a.length() != 0.0 ) && ( r != 0.0 ) ) 102 | { 103 | ofMatrix4x4 R; 104 | R.makeRotationMatrix(RAD_TO_DEG * r, a); 105 | ofMatrix4x4 Tj; 106 | Tj.makeTranslationMatrix( points.back() ); 107 | ofMatrix4x4 Ti; 108 | Ti.makeTranslationMatrix( -points[points.size() - 2] ); 109 | 110 | frames.push_back(frames.back() * Ti * R * Tj); 111 | } 112 | else 113 | { 114 | ofMatrix4x4 Tr; 115 | Tr.makeTranslationMatrix( points.back() - points[points.size() - 2] ); 116 | 117 | frames.push_back(frames.back() * Tr); 118 | } 119 | prevTangent = curTangent; 120 | while (frames.size() > maxFrames) frames.pop_front(); 121 | } 122 | 123 | ofMatrix4x4 ParallelTransportFrames::normalMatrix() const 124 | { 125 | ofMatrix4x4 normalMatrix = ofMatrix4x4::getTransposedOf(const_cast(frames.back()).getInverse()); 126 | return ofMatrix4x4(normalMatrix(0, 0), normalMatrix(0, 1), normalMatrix(0, 2), 0.f, 127 | normalMatrix(1, 0), normalMatrix(1, 1), normalMatrix(1, 2), 0.f, 128 | normalMatrix(2, 0), normalMatrix(2, 1), normalMatrix(2, 2), 0.f, 129 | 0.f, 0.f, 0.f, 1.f); 130 | } 131 | 132 | ofVec3f ParallelTransportFrames::calcCurrentNormal() const 133 | { 134 | return getStartNormal() * normalMatrix(); 135 | } 136 | 137 | void ParallelTransportFrames::debugDraw(float axisSize) 138 | { 139 | ofSetColor(0, 255, 255); 140 | ofNoFill(); 141 | for (int i = 0; i < frames.size(); ++i) 142 | { 143 | ofPushMatrix(); 144 | ofMultMatrix(frames[i]); 145 | ofRotate(90, 0, 1, 0); 146 | ofCircle(0, 0, axisSize * 2.f); 147 | ofDrawAxis(axisSize); 148 | ofPopMatrix(); 149 | } 150 | } 151 | 152 | void ParallelTransportFrames::clear() 153 | { 154 | points.clear(); 155 | frames.clear(); 156 | } 157 | } 158 | -------------------------------------------------------------------------------- /visualization/line_viz/src/ofxptf/ParallelTransportFrames.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ParallelTransportFrames.h 3 | * 4 | * Copyright (c) 2012, Neil Mendoza, http://www.neilmendoza.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of Neil Mendoza nor the names of its contributors may be used 16 | * to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | */ 32 | #pragma once 33 | 34 | #include "ofMain.h" 35 | 36 | namespace itg 37 | { 38 | /* 39 | * PTF based on Cinder's implementation as edge cases are all handled 40 | */ 41 | class ParallelTransportFrames 42 | { 43 | public: 44 | // TODO: add startNormal 45 | 46 | ParallelTransportFrames(); 47 | 48 | bool addPoint(const ofVec3f& point); 49 | void debugDraw(float axisSize = 10.f); 50 | 51 | ofMatrix4x4 transformMatrix() const { return frames.back(); } 52 | ofMatrix4x4 normalMatrix() const; 53 | 54 | unsigned framesSize() const { return frames.size(); } 55 | unsigned pointsSize() const { return points.size(); } 56 | 57 | deque& getFrames() { return frames; } 58 | 59 | ofMatrix4x4 frameAt(unsigned idx) const { return frames[idx]; } 60 | 61 | ofVec3f getStartNormal() const { return startNormal; } 62 | ofVec3f getCurrentTangent() const { return curTangent; } 63 | 64 | ofVec3f calcCurrentNormal() const; 65 | 66 | void clear(); 67 | 68 | void setMaxFrames(unsigned maxFrames) { this->maxFrames = maxFrames; } 69 | 70 | private: 71 | unsigned maxPoints, maxFrames; 72 | 73 | void firstFrame(); 74 | void nextFrame(); 75 | 76 | ofVec3f startNormal; 77 | ofVec3f prevTangent, curTangent; 78 | 79 | deque points; 80 | deque frames; 81 | }; 82 | } 83 | -------------------------------------------------------------------------------- /visualization/line_viz/src/ofxptf/ofxPtf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ofxPtf.h 3 | * 4 | * Copyright (c) 2013, Neil Mendoza, http://www.neilmendoza.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of Neil Mendoza nor the names of its contributors may be used 16 | * to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | */ 32 | #pragma once 33 | 34 | #include "ParallelTransportFrames.h" 35 | 36 | typedef itg::ParallelTransportFrames ofxPtf; 37 | -------------------------------------------------------------------------------- /visualization/line_viz/src/qpose/src/commonMath.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * commonMath.cpp 3 | * \author Joshua Vasquez 4 | * \date June 14, 2014 5 | */ 6 | #include "commonMath.hpp" 7 | 8 | //FIXME: change std::numeric_limits::infinity to a constexpr 9 | 10 | bool CommonMath::parallel(float slopeA, float slopeB) 11 | { 12 | // TODO: rounding here is probably redundant at this point. 13 | // round huge and tiny numbers to inf and zero respectively: 14 | float roundedA = round(slopeA); 15 | float roundedB = round(slopeB); 16 | 17 | // Handle special case where parallel vertical lines could round to 18 | // opposite slopes. 19 | if((roundedA == std::numeric_limits::infinity() || 20 | (roundedA == -std::numeric_limits::infinity())) && 21 | (roundedB == std::numeric_limits::infinity() || 22 | (roundedB == -std::numeric_limits::infinity()))) 23 | return true; 24 | 25 | // check if both lines are equal to within 1% 26 | if (almostEqual(roundedA, roundedB, ulp_)) 27 | return true; 28 | 29 | return false; 30 | } 31 | 32 | float CommonMath::tuneAngle(float angleInDeg) 33 | { 34 | float newAngle = angleInDeg; 35 | // Map to circle range: 36 | while (newAngle > 2*M_PI) 37 | newAngle -= 2*M_PI; 38 | while (newAngle < -2*M_PI) 39 | newAngle += 2*M_PI; 40 | 41 | // Convert to -Pi to Pi range. FIXME: should this be pi/2 to pi/2 range?? 42 | if (newAngle > M_PI) 43 | newAngle -= 2*M_PI; 44 | if (newAngle < -M_PI) 45 | newAngle += 2*M_PI; 46 | 47 | return newAngle; 48 | 49 | } 50 | 51 | 52 | float CommonMath::round(float input) 53 | { 54 | if (input > largeVal_) 55 | return std::numeric_limits::infinity(); 56 | if (input < -largeVal_) 57 | return -std::numeric_limits::infinity(); 58 | if (std::abs(input) < smallVal_) 59 | return 0.; 60 | return input; 61 | } 62 | 63 | 64 | bool CommonMath::almostEqual(float x, float y, int ulp) 65 | { 66 | // Compare all subnormal values (like infinity) exactly. 67 | /// OLD METHOD: slow! 68 | assert((isnan(x) == false) && (isnan(x) == false)); 69 | 70 | if (!std::isnormal(x) || !std::isnormal(y)) 71 | return x == y; 72 | 73 | /// NEW METHOD: avoid calls to isnormal 74 | /* 75 | if (x == y) 76 | return true; 77 | */ 78 | return std::abs(x - y) <= 79 | std::numeric_limits::epsilon() * std::abs(x + y) * ulp; 80 | } 81 | 82 | float CommonMath::pdf(float x, float mu, float sigma) 83 | { 84 | 85 | /// Apparently: inf - inf = nan. Fix that here 86 | if ( ((x == std::numeric_limits::infinity()) && 87 | (mu == std::numeric_limits::infinity())) || 88 | ((x == -std::numeric_limits::infinity()) && 89 | (mu == -std::numeric_limits::infinity())) ) 90 | return 1.0; 91 | 92 | /// note: weird constant is sqrt(2*pi) precomputed 93 | /* 94 | return (1/(sigma * (2.5066282746310002))) * 95 | exp(- (pow((x - mu), 2))/(2*sigma*sigma) ); 96 | */ 97 | 98 | return exp(- (pow((x - mu), 2)) / (2*sigma*sigma) ); 99 | } 100 | -------------------------------------------------------------------------------- /visualization/line_viz/src/qpose/src/commonMath.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * commonMath.hpp 3 | * \author Joshua Vasquez 4 | * \date June 12, 2014 5 | */ 6 | 7 | #ifndef COMMONMATH_HPP 8 | #define COMMONMATH_HPP 9 | #include 10 | #include 11 | #include 12 | namespace CommonMath { 13 | 14 | const float largeVal_ = 1000; 15 | const float smallVal_ = 0.001; 16 | const int ulp_ = 10000; 17 | 18 | /** 19 | * \fn parallel(float slopeA, slopeB) 20 | * \brief true if both lines are parallel within rounding error 21 | * \details handles special case of slopeA: +inf and slopeB: -inf 22 | */ 23 | bool parallel( float slopeA, float slopeB); 24 | 25 | /** 26 | * \fn float round(float input) 27 | * \brief rounds input to infinities or to zero for extreme values to prevent 28 | * rounding error 29 | */ 30 | float round(float input); 31 | 32 | 33 | /** 34 | * bool almostEqual( float val1, float val2, int ulp) 35 | * \brief true if two numbers are close enough (for this algorithm) to be 36 | * considered equal 37 | * \param ulp is the number of units in the last place 38 | * \details href='http://en.cppreference.com/w/cpp/types/numeric_limits/epsilon'>algorithm reference 39 | * 40 | */ 41 | bool almostEqual( float val1, float val2, int ulp); 42 | 43 | /** 44 | * \fn tuneAngle(float angle) 45 | * \brief converts input float (in radians) to the correct range of -Pi to Pi. 46 | */ 47 | float tuneAngle(float angle); 48 | 49 | /** 50 | * \fn float pdf(float x, float mu, float sigma) 51 | * \brief return the probability of getting x from a Gaussian distribution 52 | * centered at mu with std deviation sigma. 53 | */ 54 | float pdf(float x, float mu, float sigma); 55 | 56 | }; 57 | 58 | #endif // COMMONMATH_HPP 59 | -------------------------------------------------------------------------------- /visualization/line_viz/src/qpose/src/quaternion.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Templated Quaternion Class 3 | * \author Joshua Vasquez 4 | * \date August 31, 2014 5 | */ 6 | #ifndef QUATERNION_HPP 7 | #define QUATERNION_HPP 8 | 9 | #include 10 | #include 11 | #include "commonMath.hpp" 12 | 13 | 14 | 15 | /** 16 | * \class Quaternion 17 | * \brief a templated quaternion class that also enables quick storage and 18 | * retrieval of rotations encoded as a vector3 and angle. 19 | * \details All angles are in radians. 20 | * \warning This template is intended to be instantiated with a floating point 21 | * data type. 22 | */ 23 | template class Quaternion 24 | { 25 | public: 26 | Quaternion() 27 | : w_(1), x_(0), y_(0), z_(0) 28 | {} 29 | 30 | Quaternion( T w, T x, T y, T z) 31 | : w_(w), x_(x), y_(y), z_(z) 32 | {} 33 | 34 | ~Quaternion() 35 | {} 36 | 37 | 38 | /** 39 | * Quaternion Rotation Properties for straightforward usage of quaternions 40 | * to store rotations. 41 | */ 42 | 43 | /** 44 | * \fn void encodeRotation( T theta, T x, T y, T z) 45 | * \brief Store a normalized rotation in the quaternion encoded as a rotation 46 | * of theta about the vector (x,y,z). 47 | */ 48 | void encodeRotation( T theta, T x, T y, T z) 49 | { 50 | w_ = cos(theta / 2); 51 | x_ = x * sin(theta / 2); 52 | y_ = y * sin(theta / 2); 53 | z_ = z * sin(theta / 2); 54 | normalize(); 55 | } 56 | 57 | /** 58 | * \fn void getRotation( T& angle, T& x, T& y, T& z) 59 | * \brief Retrieve the rotation (angle and vector3) stored in the quaternion. 60 | * \warning only unit quaternions represent rotation. 61 | * \details A quaternion: 62 | * Q = cos(alpha) + Usin(alpha), where U is a vector3, stores a 63 | rotation 64 | * of 2*alpha about the 3D axis U. This member function retrieves 65 | theta and U, where theta = 2*alpha is the amount of rotation 66 | about the vector U. 67 | * \note the angle retrieved is in radians. 68 | */ 69 | void getRotation( T& theta, T& x, T& y, T& z) 70 | { 71 | // Acquire the amount of rotation. Prevent against rounding error. 72 | if ((w_ > 1) || (w_ < -1)) 73 | theta = 2 * acos(1); 74 | else 75 | theta = 2 * acos(w_); 76 | /// The following is 'more numerically stable' according to Wikipedia: 77 | /// but it doesn't work as well for small angles. 78 | //theta = 2 * atan2( norm(), w_); 79 | 80 | T commonVal = sin(theta /2); 81 | 82 | // Acquire rotational axis. Guard agains division by 0. 83 | if (commonVal != 0) 84 | { 85 | x = x_ / commonVal; 86 | y = y_ / commonVal; 87 | z = z_ / commonVal; 88 | } 89 | else // Guard against division by zero. Values are bogus but ignored. 90 | { 91 | x = x_; 92 | y = y_; 93 | z = z_; 94 | } 95 | } 96 | 97 | 98 | /** 99 | * \fn void rotate( T& x, T& y, T& z) 100 | * \brief rotate a vector3 (x,y,z) by the angle theta about the axis 101 | * (U_x, U_y, U_z) stored in the quaternion. 102 | */ 103 | void rotate(T& x, T& y, T& z) 104 | { 105 | Quaternion q = (*this); 106 | Quaternion qStar = (*this).conjugate(); 107 | Quaternion rotatedVal = q * Quaternion(0, x, y, z) * qStar; 108 | 109 | x = rotatedVal.x_; 110 | y = rotatedVal.y_; 111 | z = rotatedVal.z_; 112 | } 113 | 114 | // TODO: test this function! 115 | /** 116 | * \fn toRPY 117 | * \note: source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 118 | */ 119 | void toRPY(double& roll, double& pitch, double& yaw) 120 | { 121 | double ysqr = y_ * y_; 122 | 123 | // roll (x-axis rotation) 124 | double t0 = +2.0 * (w_ * x_ + y_ * z_); 125 | double t1 = +1.0 - 2.0 * (x_ * x_ + ysqr); 126 | roll = std::atan2(t0, t1); 127 | 128 | // pitch (y-axis rotation) 129 | double t2 = +2.0 * (w_ * y_ - z_ * x_); 130 | t2 = t2 > 1.0 ? 1.0 : t2; 131 | t2 = t2 < -1.0 ? -1.0 : t2; 132 | pitch = std::asin(t2); 133 | 134 | // yaw (z-axis rotation) 135 | double t3 = +2.0 * (w_ * z_ + x_ * y_); 136 | double t4 = +1.0 - 2.0 * (ysqr + z_ * z_); 137 | yaw = std::atan2(t3, t4); 138 | } 139 | 140 | /** 141 | * Quaternion Mathematical Properties 142 | * implemented below */ 143 | 144 | /// Addition 145 | Quaternion operator+(const Quaternion& q2) 146 | { 147 | return Quaternion( (w_ + q2.w_), 148 | (x_ + q2.x_), 149 | (y_ + q2.y_), 150 | (z_ + q2.z_)); 151 | } 152 | 153 | /// Subtraction 154 | Quaternion operator-(const Quaternion& q2) 155 | { 156 | return Quaternion( (w_ - q2.w_), 157 | (x_ - q2.x_), 158 | (y_ - q2.y_), 159 | (z_ - q2.z_)); 160 | } 161 | 162 | /// equality 163 | bool operator==(const Quaternion& q2) const 164 | { 165 | return (w_ == q2.w_) && (x_ == q2.x_) 166 | && (y_ == q2.y_) && (z_ == q2.z_); 167 | } 168 | 169 | /// (left) Scalar Multiplication 170 | /** 171 | * \fn template friend Quaternion operator*(const U scalar, 172 | * const Quaternion& q) 173 | * \brief implements scalar multiplication for arbitrary scalar types. 174 | */ 175 | template friend Quaternion operator*(const U scalar, 176 | const Quaternion& q) 177 | { 178 | return Quaternion( (scalar * q.w_), 179 | (scalar * q.x_), 180 | (scalar * q.y_), 181 | (scalar * q.z_)); 182 | } 183 | 184 | /// Quaternion Product 185 | Quaternion operator*(const Quaternion& q2) 186 | { 187 | return Quaternion( 188 | ((w_*q2.w_) - (x_*q2.x_) - (y_*q2.y_) - (z_*q2.z_)), 189 | ((w_*q2.x_) + (x_*q2.w_) + (y_*q2.z_) - (z_*q2.y_)), 190 | ((w_*q2.y_) - (x_*q2.z_) + (y_*q2.w_) + (z_*q2.x_)), 191 | ((w_*q2.z_) + (x_*q2.y_) - (y_*q2.x_) + (z_*q2.w_))); 192 | } 193 | 194 | /// Quaternion Power function 195 | /** 196 | * \fn static Quaternion power(const Quaternion q1, T p) 197 | * \brief perform the power operation on a quaternion 198 | * \details A quaternion Q = (w, x, y, z) may be written as the 199 | * product of a scalar and a unit quaternion: Q = N*q = 200 | * N[sin(theta) + U_x*cos(theta) + U_y*cos(theta) + U_k*cos(theta)], where N is 201 | * a scalar and U is a vector3 (U_x, U_y, U_z) representing the normalized 202 | * vector component of the original quaternion, aka: (x,y,z). Raising a 203 | * quaternion to a p._v.w*_v.y - rhs._v.x*_v.z + rhs._v.y*_v.w + rhs._v.z*_v.x,wer can be done most easily in this form. 204 | */ 205 | static Quaternion power(Quaternion q1, T p) 206 | { 207 | T magnitude = q1.norm(); 208 | 209 | Quaternion unitQuaternion = q1; 210 | unitQuaternion.normalize(); 211 | 212 | // unitQuaternion.w_ will always be less than 1, so no domain 213 | // error. 214 | T theta = acos(unitQuaternion.w_); 215 | 216 | 217 | // Perform math: 218 | // N^p * [cos(p * theta) + U*sin(p * theta)], where U is a vector. 219 | T poweredMag = pow(magnitude, p); // N^p 220 | T cospTheta = cos(p * theta); 221 | T sinpTheta = sin(p * theta); 222 | /* 223 | std::cout << "poweredMag is " << poweredMag << std::endl; 224 | std::cout << "cospTheta is " << cospTheta << std::endl; 225 | std::cout << "p * Theta is " << p * theta << std::endl; 226 | std::cout << "sinpTheta is " << sinpTheta << std::endl; 227 | */ 228 | 229 | // Note: U_x, U_y, U_z exist in normalized q1. 230 | 231 | return Quaternion( poweredMag * cospTheta, 232 | poweredMag * unitQuaternion.x_ * sinpTheta, 233 | poweredMag * unitQuaternion.y_ * sinpTheta, 234 | poweredMag * unitQuaternion.z_ * sinpTheta); 235 | } 236 | 237 | /** 238 | * \fn static T dotProduct(Quaternion q1, Quaternion q2) 239 | * \brief returns the dot product of two quaternions. 240 | */ 241 | static T dotProduct(Quaternion q1, Quaternion q2) 242 | { 243 | T result = 0.5 * ((q1.conjugate() * q2) 244 | + (q1 * q2.conjugate()) ).w_; 245 | return result; 246 | } 247 | 248 | /// Conjugate 249 | Quaternion conjugate() 250 | { 251 | return Quaternion( w_, -x_, -y_, -z_); 252 | } 253 | 254 | /// Norm 255 | T norm() 256 | { 257 | return sqrt((w_ * w_) + (x_ * x_) + (y_ * y_) + (z_ * z_)); 258 | } 259 | 260 | /// inverse 261 | Quaternion inverse() 262 | { 263 | return (1/(*this).norm()) * (*this).conjugate(); 264 | } 265 | 266 | // Normalization 267 | /** 268 | * \fn void normalize() 269 | * \brief normalizes the quaternion to magnitude 1 270 | */ 271 | void normalize() 272 | { 273 | // should never happen unless the quaternion wasn't initialized 274 | // correctly. 275 | assert( !((w_ == 0) && (x_ == 0) && (y_ == 0) && (z_ == 0))); 276 | T theNorm = norm(); 277 | assert(theNorm > 0); 278 | (*this) = (1.0/theNorm) * (*this); 279 | return; 280 | } 281 | 282 | 283 | /** 284 | * \fn static Quaternion slerp( Quaternion q1 Quaternion q2, 285 | * T percentage) 286 | * \brief return a quaternion that is a linear interpolation between q1 and q2 287 | * where percentage (from 0 to 1) defines the amount of interpolation 288 | * \details morph one quaternion into the other with constant 'velocity.' 289 | * Implementation details from Wikipedia article on Slerp. 290 | */ 291 | static Quaternion slerp( Quaternion q1, Quaternion q2, T percentage) 292 | { 293 | try 294 | { 295 | if ((percentage > 1) || (percentage < 0)) 296 | throw 0; 297 | } 298 | catch(int e) 299 | { 300 | std::cout << "error: interpolation factor outside 0 to 1 " 301 | << "bound." << std::endl; 302 | } 303 | 304 | Quaternion result; 305 | 306 | T theta = acos(dotProduct(q1, q2)); 307 | T leftCoeff; 308 | T rightCoeff; 309 | 310 | if (CommonMath::almostEqual(theta, 0, 1000)) 311 | { 312 | /// as theta --> 0, SLERP becomes LERP. 313 | /// see Wikipedia slerp article. 314 | leftCoeff = (1 - percentage); 315 | rightCoeff = percentage; 316 | } 317 | else 318 | { 319 | leftCoeff = sin((1 - percentage) * theta)/ 320 | sin(theta); 321 | rightCoeff = sin(percentage * theta) / 322 | sin(theta); 323 | } 324 | 325 | result = leftCoeff * q1 + rightCoeff * q2; 326 | result.normalize(); 327 | return result; 328 | } 329 | 330 | /** 331 | * \fn template friend std::ostream& operator << 332 | * (std::ostream& os, const Quaternion& q); 333 | * \brief a templated friend function for printing quaternions. 334 | * \details T cannot be used as dummy parameter since it would be shared by 335 | * the class, and this function is not a member function. 336 | */ 337 | template friend std::ostream& operator << (std::ostream& os, 338 | const Quaternion& q); 339 | 340 | T w_; 341 | T x_; 342 | T y_; 343 | T z_; 344 | }; 345 | 346 | 347 | 348 | template std::ostream& operator<< (std::ostream& os, 349 | const Quaternion& q) 350 | { 351 | os << "(" << q.w_ << ", " << 352 | q.x_ << ", " << 353 | q.y_ << ", " << 354 | q.z_ << ")"; 355 | return os; 356 | } 357 | 358 | 359 | #endif // QUATERNION_HPP 360 | --------------------------------------------------------------------------------