├── Arduino-Processing-code ├── Arduino │ ├── Plantar_MPU_Multiple │ │ ├── Kalman.h │ │ └── Plantar_MPU_Multiple.ino │ ├── Readme_Arduino.txt │ └── libraries │ │ ├── I2Cdev │ │ ├── I2Cdev.cpp │ │ ├── I2Cdev.h │ │ ├── keywords.txt │ │ └── library.json │ │ ├── MPU6050 │ │ ├── MPU6050.cpp │ │ ├── MPU6050.h │ │ ├── MPU6050_6Axis_MotionApps20.h │ │ ├── MPU6050_9Axis_MotionApps41.h │ │ ├── helper_3dmath.h │ │ └── library.json │ │ ├── MPU6050_DMP6_Multiple-master │ │ ├── .gitignore │ │ ├── DeathTimer.h │ │ ├── LICENSE │ │ ├── MPU6050_Wrapper.h │ │ ├── README.md │ │ └── TogglePin.h │ │ └── Readme └── Processing │ └── Soleoffline │ ├── Plantar Pressure Distribution 2.csv │ ├── Readme.txt │ └── Soleoffline.pde ├── Demo_Video.mp4 ├── Gait.png ├── Gait_Phases.png ├── Gait_cycle.jpg ├── IMU.jpg ├── Insole Visualization.gif ├── Insole.png ├── Insole_Regions_one_Gait_cycle.png ├── Kinovea.mp4 ├── Kinovea.png ├── PP.mp4 └── README.md /Arduino-Processing-code/Arduino/Plantar_MPU_Multiple/Kalman.h: -------------------------------------------------------------------------------- 1 | #ifndef _Kalman_h 2 | #define _Kalman_h 3 | 4 | class Kalman { 5 | public: 6 | Kalman() { 7 | /* We will set the variables like so, these can also be tuned by the user */ 8 | Q_angle = 0.001; 9 | Q_bias = 0.003; 10 | R_measure = 0.03; 11 | 12 | angle = 0; // Reset the angle 13 | bias = 0; // Reset bias 14 | 15 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 16 | P[0][1] = 0; 17 | P[1][0] = 0; 18 | P[1][1] = 0; 19 | }; 20 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 21 | double getAngle(double newAngle, double newRate, double dt) { 22 | // KasBot V2 - Kalman filter module 23 | // Update xhat - Project the state ahead 24 | /* Step 1 */ 25 | rate = newRate - bias; 26 | angle += dt * rate; 27 | 28 | // Update estimation error covariance - Project the error covariance ahead 29 | /* Step 2 */ 30 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 31 | P[0][1] -= dt * P[1][1]; 32 | P[1][0] -= dt * P[1][1]; 33 | P[1][1] += Q_bias * dt; 34 | 35 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 36 | // Calculate Kalman gain - Compute the Kalman gain 37 | /* Step 4 */ 38 | S = P[0][0] + R_measure; 39 | /* Step 5 */ 40 | K[0] = P[0][0] / S; 41 | K[1] = P[1][0] / S; 42 | 43 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 44 | /* Step 3 */ 45 | y = newAngle - angle; 46 | /* Step 6 */ 47 | angle += K[0] * y; 48 | bias += K[1] * y; 49 | 50 | // Calculate estimation error covariance - Update the error covariance 51 | /* Step 7 */ 52 | P[0][0] -= K[0] * P[0][0]; 53 | P[0][1] -= K[0] * P[0][1]; 54 | P[1][0] -= K[1] * P[0][0]; 55 | P[1][1] -= K[1] * P[0][1]; 56 | 57 | return angle; 58 | }; 59 | void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 60 | double getRate() { return rate; }; // Return the unbiased rate 61 | 62 | /* These are used to tune the Kalman filter */ 63 | void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 64 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 65 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 66 | 67 | double getQangle() { return Q_angle; }; 68 | double getQbias() { return Q_bias; }; 69 | double getRmeasure() { return R_measure; }; 70 | 71 | private: 72 | /* Kalman filter variables */ 73 | double Q_angle; // Process noise variance for the accelerometer 74 | double Q_bias; // Process noise variance for the gyro bias 75 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 76 | 77 | double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 78 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 79 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 80 | 81 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 82 | double K[2]; // Kalman gain - This is a 2x1 vector 83 | double y; // Angle difference 84 | double S; // Estimate error 85 | }; 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/Plantar_MPU_Multiple/Plantar_MPU_Multiple.ino: -------------------------------------------------------------------------------- 1 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 2 | // for both classes must be in the include path of your project 3 | 4 | const int S0 = 5; 5 | const int S1 = 4; 6 | const int S2 = 3; 7 | const int S3 = 2; 8 | #include 9 | 10 | #include "I2Cdev.h" 11 | #include "MPU6050_Wrapper.h" 12 | #include "TogglePin.h" 13 | #include "DeathTimer.h" 14 | #include "Kalman.h" 15 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 16 | #include "Wire.h" 17 | #endif 18 | 19 | 20 | #define OUTPUT_READABLE_EULER 21 | //#define RESTRICT_PITCH 22 | const bool useSecondMpu = true; 23 | MPU6050_Array mpus(useSecondMpu ? 2 : 1); 24 | 25 | #define AD0_PIN_0 6 // Connect this pin to the AD0 pin on MPU #0 26 | #define AD0_PIN_1 7 // Connect this pin to the AD0 pin on MPU #1 27 | 28 | Kalman kalmanX; // Create the Kalman instances IMU 1 29 | Kalman kalmanY; 30 | 31 | Kalman kalmanX1; // Create the Kalman instances IMU 2 32 | Kalman kalmanY1; 33 | 34 | /* IMU Data */ 35 | double accX1, accY1, accZ1; 36 | double gyroX1, gyroY1, gyroZ1; 37 | 38 | double accX, accY, accZ; 39 | double accXprevious, accX1previous; 40 | double gyroX, gyroY, gyroZ; 41 | 42 | int trigger=0, trigger1=0; 43 | int16_t tempRaw; 44 | int Sec20=0; 45 | 46 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 47 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 48 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 49 | 50 | double compcalAngleX=0.00, compAngleXprevious, kalcalAngleX=0.00, kalAngleXprevious; 51 | double compcalAngleX1=0.00, compAngleX1previous, kalcalAngleX1=0.00, kalAngleX1previous; 52 | 53 | double gyroXangle1, gyroYangle1; // Angle calculate using the gyro only 54 | double compAngleX1, compAngleY1; // Calculated angle using a complementary filter 55 | double kalAngleX1, kalAngleY1; // Calculated angle using a Kalman filter 56 | 57 | uint32_t timer; 58 | uint8_t i2cData[14]; // Buffer for I2C data 59 | 60 | uint32_t timer1; 61 | uint8_t i2cData1[14]; // Buffer for I2C data 62 | 63 | uint8_t fifoBuffer[64]; // FIFO storage buffer 64 | 65 | // orientation/motion vars 66 | Quaternion q; // [w, x, y, z] quaternion container 67 | VectorInt16 aa; // [x, y, z] accel sensor measurements 68 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 69 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 70 | VectorFloat gravity; // [x, y, z] gravity vector 71 | float euler[3]; // [psi, theta, phi] Euler angle container 72 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 73 | int status=1; 74 | // packet structure for InvenSense teapot demo 75 | uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' }; 76 | float theta, z, qw,qx,qz,qy, x,y,l,m, psi,phi; 77 | int timeinitial=millis(), timefinal; 78 | 79 | double roll, roll1, pitch, pitch1,gyroXrate, calroll, calroll1; 80 | 81 | double Roll_matrix[3]={0.00,0.00,0.00}; 82 | 83 | double Roll1_matrix[3]={0.00,0.00,0.00}; 84 | 85 | DeathTimer deathTimer(5000L); 86 | 87 | // ================================================================ 88 | // === INITIAL SETUP === 89 | // ================================================================ 90 | 91 | void setup() { 92 | 93 | // initialize serial communication at 9600 bits per second: 94 | Serial.begin(115200); 95 | //BluetoothSerial.begin(2000000); 96 | pinMode(S0, OUTPUT); 97 | pinMode(S1, OUTPUT); 98 | pinMode(S2, OUTPUT); 99 | pinMode(S3, OUTPUT); 100 | 101 | // join I2C bus (I2Cdev library doesn't do this automatically) 102 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 103 | Wire.begin(); 104 | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties 105 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 106 | Fastwire::setup(400, true); 107 | #endif 108 | 109 | // initialize serial communication 110 | // (115200 chosen because it is required for Teapot Demo output, but it's 111 | // really up to you depending on your project) 112 | //Serial.begin(115200); 113 | TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 114 | 115 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 116 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 117 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 118 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 119 | 120 | i2cData1[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 121 | i2cData1[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 122 | i2cData1[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 123 | i2cData1[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 124 | 125 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 126 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 127 | 128 | // while (i2cRead(0x75, i2cData, 1)); 129 | // if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 130 | // Serial.print(F("Error reading sensor")); 131 | // while (1); 132 | // } 133 | 134 | while (i2cWrite(0x19, i2cData1, 4, false)); // Write to all four registers at once 135 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 136 | 137 | //while (i2cRead(0x75, i2cData1, 1)); 138 | // if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 139 | // Serial.print(F("Error reading sensor")); 140 | // while (1); 141 | // } 142 | 143 | delay(100); // Wait for sensor to stabilize 144 | 145 | /* Set kalman and gyro starting angle */ 146 | while (i2cRead(0x3B, i2cData, 6)); 147 | accX = (i2cData[0] << 8) | i2cData[1]; 148 | accY = (i2cData[2] << 8) | i2cData[3]; 149 | accZ = (i2cData[4] << 8) | i2cData[5]; 150 | 151 | while (i2cRead(0x3B, i2cData1, 6)); 152 | accX1 = (i2cData[0] << 8) | i2cData1[1]; 153 | accY1 = (i2cData[2] << 8) | i2cData1[3]; 154 | accZ1 = (i2cData[4] << 8) | i2cData1[5]; 155 | 156 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 157 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 158 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 159 | 160 | double roll1 = atan2(accY1, accZ1) * RAD_TO_DEG; 161 | double pitch1 = atan(-accX1 / sqrt(accY1 * accY1 + accZ1 * accZ1)) * RAD_TO_DEG; 162 | #else // Eq. 28 and 29 163 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 164 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 165 | 166 | double roll1 = atan(accY1 / sqrt(accX1 * accX1 + accZ1 * accZ1)) * RAD_TO_DEG; 167 | double pitch1 = atan2(-accX1, accZ1) * RAD_TO_DEG; 168 | #endif 169 | 170 | kalmanX.setAngle(roll); // Set starting angle 171 | kalmanY.setAngle(pitch); 172 | gyroXangle = roll; 173 | gyroYangle = pitch; 174 | compAngleX = roll; 175 | compAngleY = pitch; 176 | 177 | kalmanX1.setAngle(roll); // Set starting angle 178 | kalmanY1.setAngle(pitch); 179 | gyroXangle1 = roll1; 180 | gyroYangle1 = pitch1; 181 | compAngleX1 = roll1; 182 | compAngleY1 = pitch1; 183 | 184 | timer = micros(); 185 | timer1 = micros(); 186 | 187 | while (!Serial) 188 | ; // wait for Leonardo enumeration, others continue immediately 189 | 190 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio 191 | // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to 192 | // the baud timing being too misaligned with processor ticks. You must use 193 | // 38400 or slower in these cases, or use some kind of external separate 194 | // crystal solution for the UART timer. 195 | 196 | // initialize device 197 | //Serial.println(F("Initializing I2C devices...")); 198 | mpus.add(AD0_PIN_0); 199 | if (useSecondMpu) mpus.add(AD0_PIN_1); 200 | 201 | mpus.initialize(); 202 | // load and configure the DMP 203 | //Serial.println(F("Initializing DMP...")); 204 | mpus.dmpInitialize(); 205 | 206 | // supply your own gyro offsets here, scaled for min sensitivity 207 | MPU6050_Wrapper* currentMPU = mpus.select(0); 208 | currentMPU->_mpu.setXGyroOffset(220); 209 | currentMPU->_mpu.setYGyroOffset(76); 210 | currentMPU->_mpu.setZGyroOffset(-85); 211 | currentMPU->_mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 212 | if (useSecondMpu) { 213 | currentMPU = mpus.select(1); 214 | currentMPU->_mpu.setXGyroOffset(220); 215 | currentMPU->_mpu.setYGyroOffset(76); 216 | currentMPU->_mpu.setZGyroOffset(-85); 217 | currentMPU->_mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 218 | } 219 | mpus.programDmp(0); 220 | if (useSecondMpu) 221 | mpus.programDmp(1); 222 | } 223 | 224 | 225 | void handleMPUevent(uint8_t mpu) { 226 | 227 | MPU6050_Wrapper* currentMPU = mpus.select(mpu); 228 | // reset interrupt flag and get INT_STATUS byte 229 | currentMPU->getIntStatus(); 230 | // check for overflow (this should never happen unless our code is too inefficient) 231 | if ((currentMPU->_mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) 232 | || currentMPU->_fifoCount >= 1024) { 233 | // reset so we can continue cleanly 234 | currentMPU->resetFIFO(); 235 | //Serial.println(F("FIFO overflow!")); 236 | return; 237 | } 238 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 239 | if (currentMPU->_mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) { 240 | // read and dump a packet if the queue contains more than one 241 | while (currentMPU->_fifoCount >= 2 * currentMPU->_packetSize) { 242 | // read and dump one sample 243 | //Serial.print("DUMP"); // this trace will be removed soon 244 | currentMPU->getFIFOBytes(fifoBuffer); 245 | } 246 | 247 | // read a packet from FIFO 248 | currentMPU->getFIFOBytes(fifoBuffer); 249 | 250 | #ifdef OUTPUT_READABLE_EULER 251 | // display Euler angles in degrees 252 | currentMPU->_mpu.dmpGetQuaternion(&q, fifoBuffer); 253 | currentMPU->_mpu.dmpGetEuler(euler, &q); 254 | timefinal=millis(); 255 | qw=q.w; 256 | qx=q.x; 257 | qy=q.y; 258 | qz=q.z; 259 | x = 2*(qw*qx+qz*qy); 260 | y = (qz*qz)-(qy*qy)-(qx*qx)+(qw*qw); 261 | z = -2*((qw*qy)-(qx*qz)); 262 | l = 2*((qx*qy)+(qw*qz)); 263 | m = (qz*qz)+(qy*qy)-(qx*qx)-(qw*qw); 264 | 265 | phi = (180/3.1415)*atan2(x,y); 266 | theta = (180/3.1415)*asin(z); 267 | psi = (180/3.1415)*atan2(l,m); 268 | 269 | if(status!=mpu){ 270 | if(mpu==1){ 271 | 272 | while (i2cRead(0x3B, i2cData, 14)); 273 | accX = ((i2cData[0] << 8) | i2cData[1]); 274 | accY = ((i2cData[2] << 8) | i2cData[3]); 275 | accZ = ((i2cData[4] << 8) | i2cData[5]); 276 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 277 | gyroX = (i2cData[8] << 8) | i2cData[9]; 278 | gyroY = (i2cData[10] << 8) | i2cData[11]; 279 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 280 | 281 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 282 | timer = micros(); 283 | 284 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 285 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 286 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 287 | 288 | #else // Eq. 28 and 29 289 | double roll = (atan2(accY, sqrt(accZ * accZ + accX * accX)) * RAD_TO_DEG); 290 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 291 | 292 | #endif 293 | 294 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 295 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 296 | 297 | 298 | #ifdef RESTRICT_PITCH 299 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 300 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 301 | kalmanX.setAngle(roll); 302 | compAngleX = roll; 303 | kalAngleX = roll; 304 | gyroXangle = roll; 305 | } else 306 | {kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 307 | } 308 | if (abs(kalAngleX) > 90) 309 | gyroYrate = -gyroYrate;// Invert rate, so it fits the restriced accelerometer reading 310 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 311 | 312 | 313 | #else 314 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 315 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 316 | kalmanY.setAngle(pitch); 317 | compAngleY = pitch; 318 | kalAngleY = pitch; 319 | gyroYangle = pitch; 320 | } else 321 | {kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 322 | } 323 | 324 | if (abs(kalAngleY) > 90) 325 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 326 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 327 | #endif 328 | 329 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 330 | gyroYangle += gyroYrate * dt; 331 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 332 | //gyroYangle += kalmanY.getRate() * dt; 333 | 334 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 335 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 336 | calroll=roll; 337 | 338 | if (gyroXangle < -180 || gyroXangle > 180) 339 | gyroXangle = kalAngleX; 340 | if (gyroYangle < -180 || gyroYangle > 180) 341 | gyroYangle = kalAngleY; 342 | 343 | 344 | if(Sec20==1){ 345 | if((calroll1>compAngleX1)&&(accX1>0)) 346 | {trigger1=60;} 347 | 348 | if((calroll1>compAngleX1)&&(accX1<0)) 349 | {trigger1=0;} 350 | 351 | if(trigger1==0){compcalAngleX1=compcalAngleX1+(compAngleX1-compAngleX1previous);kalcalAngleX1=kalcalAngleX1+(kalAngleX1-kalAngleX1previous);} 352 | 353 | if(trigger1==60){compcalAngleX1=compcalAngleX1+(compAngleX1previous-compAngleX1);kalcalAngleX1=kalcalAngleX1+(kalAngleX1previous-kalAngleX1);} 354 | } 355 | 356 | // ================================================================ 357 | // === IMU 2 PRINT ANGLES === 358 | // ================================================================ 359 | 360 | Serial.print(compcalAngleX1); Serial.println(";"); 361 | Serial.println(kalcalAngleX1); 362 | 363 | 364 | accX1previous=accX; 365 | compAngleX1previous=compAngleX1; 366 | kalAngleX1previous=kalAngleX1; 367 | 368 | } 369 | else if(mpu==0){ 370 | 371 | while (i2cRead(0x3B, i2cData1, 14)); 372 | accX1 = ((i2cData1[0] << 8) | i2cData1[1]); 373 | accY1 = ((i2cData1[2] << 8) | i2cData1[3]); 374 | accZ1 = ((i2cData1[4] << 8) | i2cData1[5]); 375 | tempRaw = (i2cData1[6] << 8) | i2cData1[7]; 376 | gyroX1 = (i2cData1[8] << 8) | i2cData1[9]; 377 | gyroY1 = (i2cData1[10] << 8) | i2cData1[11]; 378 | gyroZ1 = (i2cData1[12] << 8) | i2cData1[13]; 379 | 380 | double dt1 = (double)(micros() - timer1) / 1000000; // Calculate delta time 381 | timer1 = micros(); 382 | 383 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 384 | 385 | double roll1 = atan2(accY1, accZ1) * RAD_TO_DEG; 386 | double pitch1 = atan(-accX1 / sqrt(accY1 * accY1 + accZ1 * accZ1)) * RAD_TO_DEG; 387 | 388 | #else // Eq. 28 and 29 389 | 390 | double roll1 = atan2(accY1, sqrt(accX1 * accX1 + accZ1 * accZ1)) * RAD_TO_DEG; 391 | double pitch1 = atan2(-accX1, accZ1) * RAD_TO_DEG; 392 | #endif 393 | 394 | double gyroXrate1 = gyroX1 / 131.0; // Convert to deg/s 395 | double gyroYrate1 = gyroY1 / 131.0; // Convert to deg/s 396 | 397 | 398 | 399 | #ifdef RESTRICT_PITCH 400 | 401 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 402 | if ((roll1 < -90 && kalAngleX1 > 90) || (roll1 > 90 && kalAngleX1 < -90)) { 403 | kalmanX1.setAngle(roll1); 404 | compAngleX1 = roll1; 405 | kalAngleX1 = roll1; 406 | gyroXangle1 = roll1; 407 | } else 408 | kalAngleX1 = kalmanX1.getAngle(roll1, gyroXrate1, dt1); // Calculate the angle using a Kalman filter 409 | 410 | if (abs(kalAngleX1) > 90) 411 | gyroYrate1 = -gyroYrate1; // Invert rate, so it fits the restriced accelerometer reading 412 | kalAngleY1 = kalmanY1.getAngle(pitch1, gyroYrate1, dt1); 413 | 414 | #else 415 | 416 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 417 | if ((pitch1 < -90 && kalAngleY1 > 90) || (pitch1 > 90 && kalAngleY1 < -90)) { 418 | kalmanY1.setAngle(pitch1); 419 | compAngleY1 = pitch1; 420 | kalAngleY1 = pitch1; 421 | gyroYangle1 = pitch1; 422 | } else 423 | kalAngleY1 = kalmanY1.getAngle(pitch1, gyroYrate1, dt1); // Calculate the angle using a Kalman filter 424 | 425 | if (abs(kalAngleY1) > 90) 426 | gyroXrate1 = -gyroXrate1; // Invert rate, so it fits the restriced accelerometer reading 427 | kalAngleX1 = kalmanX1.getAngle(roll1, gyroXrate1, dt1); // Calculate the angle using a Kalman filter 428 | 429 | #endif 430 | 431 | 432 | gyroXangle1 += gyroXrate1 * dt1; // Calculate gyro angle without any filter 433 | gyroYangle1 += gyroYrate1 * dt1; 434 | //gyroXangle1 += kalmanX1.getRate() * dt1; // Calculate gyro angle using the unbiased rate 435 | //gyroYangle1 += kalmanY1.getRate() * dt1; 436 | 437 | compAngleX1 = 0.93 * (compAngleX1 + gyroXrate1 * dt1) + 0.07 * roll1; // Calculate the angle using a Complimentary filter 438 | compAngleY1 = 0.93 * (compAngleY1 + gyroYrate1 * dt1) + 0.07 * pitch1; 439 | calroll1=roll1; 440 | 441 | if (gyroXangle1 < -180 || gyroXangle1 > 180) 442 | gyroXangle1 = kalAngleX1; 443 | if (gyroYangle1 < -180 || gyroYangle1 > 180) 444 | gyroYangle1 = kalAngleY1; 445 | 446 | if(((timefinal-timeinitial)>20000)){ 447 | Sec20=1; 448 | } 449 | if(((Sec20==1)&&(psiref0==0))){ 450 | phiref0=phi; 451 | if(phiref0>100){phi0positive=1;} 452 | if(phiref0<-100){phi0negative=1;} 453 | 454 | thetaref0=theta; 455 | if(thetaref0>100){theta0positive=1;} 456 | if(thetaref0<-100){theta0negative=1;} 457 | 458 | psiref0=psi; 459 | if(psiref0>100){psi0positive=1;} 460 | if(psiref0<-100){psi0negative=1;} 461 | } 462 | 463 | 464 | if((phi0positive==1)&&(phi<0)){phiangle=360+phi-phiref0;} 465 | 466 | else if((phi0negative==1)&&(phi>0)){phiangle=-360+phi-phiref0;} 467 | 468 | else{phiangle=phi-phiref0;} 469 | 470 | 471 | if((theta0positive==1)&&(theta<0)){thetaangle=360+theta-thetaref0;} 472 | 473 | else if((theta0negative==1)&&(theta>0)){thetaangle=-360+theta-thetaref0;} 474 | 475 | else{thetaangle=theta-thetaref0;} 476 | 477 | 478 | if((psi0positive==1)&&(psi<0)){psiangle=360+psi-psiref0;} 479 | 480 | else if((psi0negative==1)&&(psi>0)){psiangle=-360+psi-psiref0;} 481 | 482 | else{psiangle=psi-psiref0;} 483 | 484 | 485 | if(Sec20==1){ 486 | if((calroll>compAngleX)&&(accX>0)) 487 | {trigger=60;} 488 | 489 | if((calroll>compAngleX)&&(accX<0)) 490 | {trigger=0;} 491 | 492 | if(trigger==0){compcalAngleX=compcalAngleX+(compAngleX-compAngleXprevious);kalcalAngleX=kalcalAngleX+(kalAngleX-kalAngleXprevious);} 493 | 494 | if(trigger==60){compcalAngleX=compcalAngleX+(compAngleXprevious-compAngleX);kalcalAngleX=kalcalAngleX+(kalAngleXprevious-kalAngleX);} 495 | } 496 | 497 | // ================================================================ 498 | // === PLANTAR PRESSURE VOLTAGES === 499 | // ================================================================ 500 | 501 | //1st Row 502 | digitalWrite(S0, HIGH); 503 | digitalWrite(S1, LOW); 504 | digitalWrite(S2, LOW); 505 | digitalWrite(S3, LOW); 506 | 507 | // read the input on analog pin 0: 508 | int sensorValue1 = analogRead(A0); 509 | int sensorValue2 = analogRead(A1); 510 | int sensorValue3 = analogRead(A2); 511 | int sensorValue4 = analogRead(A3); 512 | int sensorValue5 = analogRead(A6); 513 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 514 | float voltage1 = sensorValue1 * (5.0 / 1023.0); 515 | float voltage2 = sensorValue2 * (5.0 / 1023.0); 516 | float voltage3 = sensorValue3 * (5.0 / 1023.0); 517 | float voltage4 = sensorValue4 * (5.0 / 1023.0); 518 | float voltage5 = sensorValue5 * (5.0 / 1023.0); 519 | // print out the value you read: 520 | //Serial.print(voltage1);Serial.print(","); 521 | 522 | Serial.print(millis());Serial.print(","); 523 | Serial.print(voltage2);Serial.print(","); 524 | Serial.print(voltage3);Serial.print(","); 525 | Serial.print(voltage4);Serial.print(","); 526 | // Serial.print(voltage5);Serial.print(","); 527 | 528 | //2nd row 529 | digitalWrite(S0, LOW); 530 | digitalWrite(S1, HIGH); 531 | digitalWrite(S2, LOW); 532 | digitalWrite(S3, LOW); 533 | 534 | sensorValue1 = analogRead(A0); 535 | sensorValue2 = analogRead(A1); 536 | sensorValue3 = analogRead(A2); 537 | sensorValue4 = analogRead(A3); 538 | sensorValue5 = analogRead(A6); 539 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 540 | voltage1 = sensorValue1 * (5.0 / 1023.0); 541 | voltage2 = sensorValue2 * (5.0 / 1023.0); 542 | voltage3 = sensorValue3 * (5.0 / 1023.0); 543 | voltage4 = sensorValue4 * (5.0 / 1023.0); 544 | voltage5 = sensorValue5 * (5.0 / 1023.0); 545 | // print out the value you read: 546 | Serial.print(voltage1);Serial.print(","); 547 | Serial.print(voltage2);Serial.print(","); 548 | Serial.print(voltage3);Serial.print(","); 549 | Serial.print(voltage4);Serial.print(","); 550 | Serial.print(voltage5);Serial.print(","); 551 | 552 | //3rd row 553 | digitalWrite(S0, HIGH); 554 | digitalWrite(S1, HIGH); 555 | digitalWrite(S2, LOW); 556 | digitalWrite(S3, LOW); 557 | 558 | sensorValue1 = analogRead(A0); 559 | sensorValue2 = analogRead(A1); 560 | sensorValue3 = analogRead(A2); 561 | sensorValue4 = analogRead(A3); 562 | sensorValue5 = analogRead(A6); 563 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 564 | voltage1 = sensorValue1 * (5.0 / 1023.0); 565 | voltage2 = sensorValue2 * (5.0 / 1023.0); 566 | voltage3 = sensorValue3 * (5.0 / 1023.0); 567 | voltage4 = sensorValue4 * (5.0 / 1023.0); 568 | voltage5 = sensorValue5 * (5.0 / 1023.0); 569 | // print out the value you read: 570 | Serial.print(voltage1);Serial.print(","); 571 | Serial.print(voltage2);Serial.print(","); 572 | Serial.print(voltage3);Serial.print(","); 573 | Serial.print(voltage4);Serial.print(","); 574 | Serial.print(voltage5); Serial.print(","); 575 | 576 | //4th row 577 | digitalWrite(S0, LOW); 578 | digitalWrite(S1, LOW); 579 | digitalWrite(S2, HIGH); 580 | digitalWrite(S3, LOW); 581 | 582 | sensorValue1 = analogRead(A0); 583 | sensorValue2 = analogRead(A1); 584 | sensorValue3 = analogRead(A2); 585 | sensorValue4 = analogRead(A3); 586 | sensorValue5 = analogRead(A6); 587 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 588 | voltage1 = sensorValue1 * (5.0 / 1023.0); 589 | voltage2 = sensorValue2 * (5.0 / 1023.0); 590 | voltage3 = sensorValue3 * (5.0 / 1023.0); 591 | voltage4 = sensorValue4 * (5.0 / 1023.0); 592 | voltage5 = sensorValue5 * (5.0 / 1023.0); 593 | // print out the value you read: 594 | Serial.print(voltage1);Serial.print(","); 595 | Serial.print(voltage2);Serial.print(","); 596 | Serial.print(voltage3);Serial.print(","); 597 | Serial.print(voltage4);Serial.print(","); 598 | Serial.print(voltage5);Serial.print(","); 599 | 600 | //5th row 601 | digitalWrite(S0, HIGH); 602 | digitalWrite(S1, LOW); 603 | digitalWrite(S2, HIGH); 604 | digitalWrite(S3, LOW); 605 | 606 | sensorValue1 = analogRead(A0); 607 | sensorValue2 = analogRead(A1); 608 | sensorValue3 = analogRead(A2); 609 | sensorValue4 = analogRead(A3); 610 | sensorValue5 = analogRead(A6); 611 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 612 | voltage1 = sensorValue1 * (5.0 / 1023.0); 613 | voltage2 = sensorValue2 * (5.0 / 1023.0); 614 | voltage3 = sensorValue3 * (5.0 / 1023.0); 615 | voltage4 = sensorValue4 * (5.0 / 1023.0); 616 | voltage5 = sensorValue5 * (5.0 / 1023.0); 617 | // print out the value you read: 618 | Serial.print(voltage1);Serial.print(","); 619 | Serial.print(voltage2);Serial.print(","); 620 | Serial.print(voltage3);Serial.print(","); 621 | Serial.print(voltage4);Serial.print(","); 622 | Serial.print(voltage5);Serial.print(","); 623 | 624 | //6th row 625 | digitalWrite(S0, LOW); 626 | digitalWrite(S1, HIGH); 627 | digitalWrite(S2, HIGH); 628 | digitalWrite(S3, LOW); 629 | 630 | sensorValue1 = analogRead(A0); 631 | sensorValue2 = analogRead(A1); 632 | sensorValue3 = analogRead(A2); 633 | sensorValue4 = analogRead(A3); 634 | sensorValue5 = analogRead(A6); 635 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 636 | voltage1 = sensorValue1 * (5.0 / 1023.0); 637 | voltage2 = sensorValue2 * (5.0 / 1023.0); 638 | voltage3 = sensorValue3 * (5.0 / 1023.0); 639 | voltage4 = sensorValue4 * (5.0 / 1023.0); 640 | voltage5 = sensorValue5 * (5.0 / 1023.0); 641 | // print out the value you read: 642 | Serial.print(voltage1);Serial.print(","); 643 | Serial.print(voltage2);Serial.print(","); 644 | Serial.print(voltage3);Serial.print(","); 645 | Serial.print(voltage4);Serial.print(","); 646 | Serial.print(voltage5);Serial.print(","); 647 | 648 | //7th row 649 | digitalWrite(S0, HIGH); 650 | digitalWrite(S1, HIGH); 651 | digitalWrite(S2, HIGH); 652 | digitalWrite(S3, LOW); 653 | 654 | sensorValue1 = analogRead(A0); 655 | sensorValue2 = analogRead(A1); 656 | sensorValue3 = analogRead(A2); 657 | sensorValue4 = analogRead(A3); 658 | sensorValue5 = analogRead(A6); 659 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 660 | voltage1 = sensorValue1 * (5.0 / 1023.0); 661 | voltage2 = sensorValue2 * (5.0 / 1023.0); 662 | voltage3 = sensorValue3 * (5.0 / 1023.0); 663 | voltage4 = sensorValue4 * (5.0 / 1023.0); 664 | voltage5 = sensorValue5 * (5.0 / 1023.0); 665 | // print out the value you read: 666 | Serial.print(voltage1);Serial.print(","); 667 | Serial.print(voltage2);Serial.print(","); 668 | Serial.print(voltage3);Serial.print(","); 669 | Serial.print(voltage4);Serial.print(","); 670 | Serial.print(voltage5);Serial.print(","); 671 | 672 | //8th row 673 | digitalWrite(S0, LOW); 674 | digitalWrite(S1, LOW); 675 | digitalWrite(S2, LOW); 676 | digitalWrite(S3, HIGH); 677 | 678 | //sensorValue1 = analogRead(A0); 679 | sensorValue2 = analogRead(A1); 680 | sensorValue3 = analogRead(A2); 681 | sensorValue4 = analogRead(A3); 682 | sensorValue5 = analogRead(A6); 683 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 684 | // voltage1 = sensorValue1 * (5.0 / 1023.0); 685 | voltage2 = sensorValue2 * (5.0 / 1023.0); 686 | voltage3 = sensorValue3 * (5.0 / 1023.0); 687 | voltage4 = sensorValue4 * (5.0 / 1023.0); 688 | voltage5 = sensorValue5 * (5.0 / 1023.0); 689 | // print out the value you read: 690 | //Serial.print(voltage1);Serial.print(","); 691 | Serial.print(voltage2);Serial.print(","); 692 | Serial.print(voltage3);Serial.print(","); 693 | Serial.print(voltage4);Serial.print(","); 694 | Serial.print(voltage5);Serial.print(","); 695 | 696 | //9th row 697 | digitalWrite(S0, HIGH); 698 | digitalWrite(S1, LOW); 699 | digitalWrite(S2, LOW); 700 | digitalWrite(S3, HIGH); 701 | 702 | //sensorValue1 = analogRead(A0); 703 | sensorValue2 = analogRead(A1); 704 | sensorValue3 = analogRead(A2); 705 | sensorValue4 = analogRead(A3); 706 | sensorValue5 = analogRead(A6); 707 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 708 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 709 | voltage2 = sensorValue2 * (5.0 / 1023.0); 710 | voltage3 = sensorValue3 * (5.0 / 1023.0); 711 | voltage4 = sensorValue4 * (5.0 / 1023.0); 712 | voltage5 = sensorValue5 * (5.0 / 1023.0); 713 | // print out the value you read: 714 | //Serial.print(voltage1);Serial.print(","); 715 | Serial.print(voltage2);Serial.print(","); 716 | Serial.print(voltage3);Serial.print(","); 717 | Serial.print(voltage4);Serial.print(","); 718 | Serial.print(voltage5);Serial.print(","); 719 | 720 | //10th row 721 | digitalWrite(S0, LOW); 722 | digitalWrite(S1, HIGH); 723 | digitalWrite(S2, LOW); 724 | digitalWrite(S3, HIGH); 725 | 726 | // sensorValue1 = analogRead(A0); 727 | sensorValue2 = analogRead(A1); 728 | sensorValue3 = analogRead(A2); 729 | sensorValue4 = analogRead(A3); 730 | sensorValue5 = analogRead(A6); 731 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 732 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 733 | voltage2 = sensorValue2 * (5.0 / 1023.0); 734 | voltage3 = sensorValue3 * (5.0 / 1023.0); 735 | voltage4 = sensorValue4 * (5.0 / 1023.0); 736 | voltage5 = sensorValue5 * (5.0 / 1023.0); 737 | // print out the value you read: 738 | //Serial.print(voltage1);Serial.print(","); 739 | Serial.print(voltage2);Serial.print(","); 740 | Serial.print(voltage3);Serial.print(","); 741 | Serial.print(voltage4);Serial.print(","); 742 | Serial.print(voltage5);Serial.print(","); 743 | 744 | //11th row 745 | digitalWrite(S0, HIGH); 746 | digitalWrite(S1, HIGH); 747 | digitalWrite(S2, LOW); 748 | digitalWrite(S3, HIGH); 749 | 750 | //sensorValue1 = analogRead(A0); 751 | sensorValue2 = analogRead(A1); 752 | sensorValue3 = analogRead(A2); 753 | sensorValue4 = analogRead(A3); 754 | sensorValue5 = analogRead(A6); 755 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 756 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 757 | voltage2 = sensorValue2 * (5.0 / 1023.0); 758 | voltage3 = sensorValue3 * (5.0 / 1023.0); 759 | voltage4 = sensorValue4 * (5.0 / 1023.0); 760 | voltage5 = sensorValue5 * (5.0 / 1023.0); 761 | // print out the value you read: 762 | //Serial.print(voltage1);Serial.print(","); 763 | Serial.print(voltage2);Serial.print(","); 764 | Serial.print(voltage3);Serial.print(","); 765 | Serial.print(voltage4);Serial.print(","); 766 | Serial.print(voltage5);Serial.print(","); 767 | 768 | //12th row 769 | digitalWrite(S0, LOW); 770 | digitalWrite(S1, LOW); 771 | digitalWrite(S2, HIGH); 772 | digitalWrite(S3, HIGH); 773 | 774 | //sensorValue1 = analogRead(A0); 775 | sensorValue2 = analogRead(A1); 776 | sensorValue3 = analogRead(A2); 777 | sensorValue4 = analogRead(A3); 778 | sensorValue5 = analogRead(A6); 779 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 780 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 781 | voltage2 = sensorValue2 * (5.0 / 1023.0); 782 | voltage3 = sensorValue3 * (5.0 / 1023.0); 783 | voltage4 = sensorValue4 * (5.0 / 1023.0); 784 | voltage5 = sensorValue5 * (5.0 / 1023.0); 785 | // print out the value you read: 786 | //Serial.print(voltage1);Serial.print(","); 787 | Serial.print(voltage2);Serial.print(","); 788 | Serial.print(voltage3);Serial.print(","); 789 | Serial.print(voltage4);Serial.print(","); 790 | Serial.print(voltage5);Serial.print(","); 791 | 792 | //13th row 793 | digitalWrite(S0, HIGH); 794 | digitalWrite(S1, LOW); 795 | digitalWrite(S2, HIGH); 796 | digitalWrite(S3, HIGH); 797 | 798 | //sensorValue1 = analogRead(A0); 799 | sensorValue2 = analogRead(A1); 800 | sensorValue3 = analogRead(A2); 801 | sensorValue4 = analogRead(A3); 802 | sensorValue5 = analogRead(A6); 803 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 804 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 805 | voltage2 = sensorValue2 * (5.0 / 1023.0); 806 | voltage3 = sensorValue3 * (5.0 / 1023.0); 807 | voltage4 = sensorValue4 * (5.0 / 1023.0); 808 | voltage5 = sensorValue5 * (5.0 / 1023.0); 809 | // print out the value you read: 810 | //Serial.print(voltage1);Serial.print(","); 811 | Serial.print(voltage2);Serial.print(","); 812 | Serial.print(voltage3);Serial.print(","); 813 | Serial.print(voltage4);Serial.print(","); 814 | Serial.print(voltage5);Serial.print(","); 815 | 816 | //14th row 817 | digitalWrite(S0, LOW); 818 | digitalWrite(S1, HIGH); 819 | digitalWrite(S2, HIGH); 820 | digitalWrite(S3, HIGH); 821 | 822 | //sensorValue1 = analogRead(A0); 823 | sensorValue2 = analogRead(A1); 824 | sensorValue3 = analogRead(A2); 825 | sensorValue4 = analogRead(A3); 826 | sensorValue5 = analogRead(A6); 827 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 828 | //voltage1 = sensorValue1 * (5.0 / 1023.0); 829 | voltage2 = sensorValue2 * (5.0 / 1023.0); 830 | voltage3 = sensorValue3 * (5.0 / 1023.0); 831 | voltage4 = sensorValue4 * (5.0 / 1023.0); 832 | voltage5 = sensorValue5 * (5.0 / 1023.0); 833 | // print out the value you read: 834 | //Serial.print(voltage1);Serial.print(","); 835 | Serial.print(voltage2);Serial.print(","); 836 | Serial.print(voltage3);Serial.print(","); 837 | Serial.print(voltage4);Serial.print(","); 838 | Serial.print(voltage5);Serial.print(","); 839 | 840 | //15th row 841 | digitalWrite(S0, HIGH); 842 | digitalWrite(S1, HIGH); 843 | digitalWrite(S2, HIGH); 844 | digitalWrite(S3, HIGH); 845 | 846 | //sensorValue1 = analogRead(A0); 847 | sensorValue2 = analogRead(A1); 848 | sensorValue3 = analogRead(A2); 849 | sensorValue4 = analogRead(A3); 850 | //sensorValue5 = analogRead(A6); 851 | // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V): 852 | // voltage1 = sensorValue1 * (5.0 / 1023.0); 853 | voltage2 = sensorValue2 * (5.0 / 1023.0); 854 | voltage3 = sensorValue3 * (5.0 / 1023.0); 855 | voltage4 = sensorValue4 * (5.0 / 1023.0); 856 | // voltage5 = sensorValue5 * (5.0 / 1023.0); 857 | // print out the value you read: 858 | //Serial.print(voltage1);Serial.print(","); 859 | Serial.print(voltage2);Serial.print(","); 860 | Serial.print(voltage3);Serial.print(","); 861 | Serial.print(voltage4);Serial.print(";"); 862 | //Serial.println(voltage5); 863 | //BlueTooth.write(".."); 864 | 865 | // ================================================================ 866 | // === IMU 1 PRINT ANGLES === 867 | // ================================================================ 868 | 869 | Serial.print(compcalAngleX); Serial.print(";"); 870 | Serial.print(kalcalAngleX); Serial.print(";"); 871 | 872 | accXprevious=accX; 873 | compAngleXprevious=compAngleX; 874 | kalAngleXprevious=kalAngleX; 875 | } 876 | status=mpu; 877 | } 878 | #endif 879 | } 880 | } 881 | 882 | // ================================================================ 883 | // === MAIN PROGRAM LOOP === 884 | // ================================================================ 885 | 886 | void loop() { 887 | 888 | static uint8_t mpu = 0; 889 | static MPU6050_Wrapper* currentMPU = NULL; 890 | if (useSecondMpu) { 891 | for (int i=0;i<2;i++) { 892 | mpu=(mpu+1)%2; // failed attempt at round robin 893 | currentMPU = mpus.select(mpu); 894 | if (currentMPU->isDue()) { 895 | handleMPUevent(mpu); 896 | } 897 | } 898 | } else { 899 | mpu=0; 900 | currentMPU = mpus.select(mpu); 901 | if (currentMPU->isDue()) { 902 | handleMPUevent(mpu); 903 | } 904 | } 905 | } 906 | 907 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/Readme_Arduino.txt: -------------------------------------------------------------------------------- 1 | Instructions - 2 | 3 | 1) Place the contents of the libraries folder in location C:\Users\User_Name\Documents\Arduino\libraries 4 | 5 | 2) Upload the Plantar_MPU_Multiple code onto the Arduino Board via USB cable ("Note: Check COM port") 6 | 7 | 3) Connect the Arduino board, Sensors to the buit cicuit and power via Power bank (5V) 8 | 9 | 4) Pair Bluetooth of the computer in use to onboard Bluetooth module (HC05) 10 | 11 | 5) Open Processing Folder and use the files accordingly -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/I2Cdev/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 2013-06-05 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 7 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 8 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 9 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 10 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 11 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 12 | // 2011-10-03 - added automatic Arduino version detection for ease of use 13 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 14 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 15 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 16 | // 2011-08-02 - added support for 16-bit registers 17 | // - fixed incorrect Doxygen comments on some methods 18 | // - added timeout value for read operations (thanks mem @ Arduino forums) 19 | // 2011-07-30 - changed read/write function structures to return success or byte counts 20 | // - made all methods static for multi-device memory savings 21 | // 2011-07-28 - initial release 22 | 23 | /* ============================================ 24 | I2Cdev device library code is placed under the MIT license 25 | Copyright (c) 2013 Jeff Rowberg 26 | 27 | Permission is hereby granted, free of charge, to any person obtaining a copy 28 | of this software and associated documentation files (the "Software"), to deal 29 | in the Software without restriction, including without limitation the rights 30 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 31 | copies of the Software, and to permit persons to whom the Software is 32 | furnished to do so, subject to the following conditions: 33 | 34 | The above copyright notice and this permission notice shall be included in 35 | all copies or substantial portions of the Software. 36 | 37 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 38 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 39 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 40 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 41 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 42 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 43 | THE SOFTWARE. 44 | =============================================== 45 | */ 46 | 47 | #ifndef _I2CDEV_H_ 48 | #define _I2CDEV_H_ 49 | 50 | // ----------------------------------------------------------------------------- 51 | // I2C interface implementation setting 52 | // ----------------------------------------------------------------------------- 53 | #ifndef I2CDEV_IMPLEMENTATION 54 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 55 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 56 | #endif // I2CDEV_IMPLEMENTATION 57 | 58 | // comment this out if you are using a non-optimal IDE/implementation setting 59 | // but want the compiler to shut up about it 60 | #define I2CDEV_IMPLEMENTATION_WARNINGS 61 | 62 | // ----------------------------------------------------------------------------- 63 | // I2C interface implementation options 64 | // ----------------------------------------------------------------------------- 65 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 66 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 67 | // ^^^ NBWire implementation is still buggy w/some interrupts! 68 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 69 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 70 | 71 | // ----------------------------------------------------------------------------- 72 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 73 | // ----------------------------------------------------------------------------- 74 | //#define I2CDEV_SERIAL_DEBUG 75 | 76 | #ifdef ARDUINO 77 | #if ARDUINO < 100 78 | #include "WProgram.h" 79 | #else 80 | #include "Arduino.h" 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 83 | #include 84 | #endif 85 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 86 | #include 87 | #endif 88 | #endif 89 | 90 | #ifdef SPARK 91 | #include 92 | #define ARDUINO 101 93 | #endif 94 | 95 | 96 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 97 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 98 | 99 | class I2Cdev { 100 | public: 101 | I2Cdev(); 102 | 103 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 104 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 105 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 106 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 107 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 108 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 109 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 110 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 111 | 112 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 113 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 114 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 115 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 116 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 117 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 118 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 119 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 120 | 121 | static uint16_t readTimeout; 122 | }; 123 | 124 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 125 | ////////////////////// 126 | // FastWire 0.24 127 | // This is a library to help faster programs to read I2C devices. 128 | // Copyright(C) 2012 129 | // Francesco Ferrara 130 | ////////////////////// 131 | 132 | /* Master */ 133 | #define TW_START 0x08 134 | #define TW_REP_START 0x10 135 | 136 | /* Master Transmitter */ 137 | #define TW_MT_SLA_ACK 0x18 138 | #define TW_MT_SLA_NACK 0x20 139 | #define TW_MT_DATA_ACK 0x28 140 | #define TW_MT_DATA_NACK 0x30 141 | #define TW_MT_ARB_LOST 0x38 142 | 143 | /* Master Receiver */ 144 | #define TW_MR_ARB_LOST 0x38 145 | #define TW_MR_SLA_ACK 0x40 146 | #define TW_MR_SLA_NACK 0x48 147 | #define TW_MR_DATA_ACK 0x50 148 | #define TW_MR_DATA_NACK 0x58 149 | 150 | #define TW_OK 0 151 | #define TW_ERROR 1 152 | 153 | class Fastwire { 154 | private: 155 | static boolean waitInt(); 156 | 157 | public: 158 | static void setup(int khz, boolean pullup); 159 | static byte beginTransmission(byte device); 160 | static byte write(byte value); 161 | static byte writeBuf(byte device, byte address, byte *data, byte num); 162 | static byte readBuf(byte device, byte address, byte *data, byte num); 163 | static void reset(); 164 | static byte stop(); 165 | }; 166 | #endif 167 | 168 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 169 | // NBWire implementation based heavily on code by Gene Knight 170 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 171 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 172 | 173 | #define NBWIRE_BUFFER_LENGTH 32 174 | 175 | class TwoWire { 176 | private: 177 | static uint8_t rxBuffer[]; 178 | static uint8_t rxBufferIndex; 179 | static uint8_t rxBufferLength; 180 | 181 | static uint8_t txAddress; 182 | static uint8_t txBuffer[]; 183 | static uint8_t txBufferIndex; 184 | static uint8_t txBufferLength; 185 | 186 | // static uint8_t transmitting; 187 | static void (*user_onRequest)(void); 188 | static void (*user_onReceive)(int); 189 | static void onRequestService(void); 190 | static void onReceiveService(uint8_t*, int); 191 | 192 | public: 193 | TwoWire(); 194 | void begin(); 195 | void begin(uint8_t); 196 | void begin(int); 197 | void beginTransmission(uint8_t); 198 | //void beginTransmission(int); 199 | uint8_t endTransmission(uint16_t timeout=0); 200 | void nbendTransmission(void (*function)(int)) ; 201 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 202 | //uint8_t requestFrom(int, int); 203 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 204 | void send(uint8_t); 205 | void send(uint8_t*, uint8_t); 206 | //void send(int); 207 | void send(char*); 208 | uint8_t available(void); 209 | uint8_t receive(void); 210 | void onReceive(void (*)(int)); 211 | void onRequest(void (*)(void)); 212 | }; 213 | 214 | #define TWI_READY 0 215 | #define TWI_MRX 1 216 | #define TWI_MTX 2 217 | #define TWI_SRX 3 218 | #define TWI_STX 4 219 | 220 | #define TW_WRITE 0 221 | #define TW_READ 1 222 | 223 | #define TW_MT_SLA_NACK 0x20 224 | #define TW_MT_DATA_NACK 0x30 225 | 226 | #define CPU_FREQ 16000000L 227 | #define TWI_FREQ 100000L 228 | #define TWI_BUFFER_LENGTH 32 229 | 230 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 231 | 232 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 233 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 234 | #define TW_START 0x08 235 | #define TW_REP_START 0x10 236 | #define TW_MT_SLA_ACK 0x18 237 | #define TW_MT_SLA_NACK 0x20 238 | #define TW_MT_DATA_ACK 0x28 239 | #define TW_MT_DATA_NACK 0x30 240 | #define TW_MT_ARB_LOST 0x38 241 | #define TW_MR_ARB_LOST 0x38 242 | #define TW_MR_SLA_ACK 0x40 243 | #define TW_MR_SLA_NACK 0x48 244 | #define TW_MR_DATA_ACK 0x50 245 | #define TW_MR_DATA_NACK 0x58 246 | #define TW_ST_SLA_ACK 0xA8 247 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 248 | #define TW_ST_DATA_ACK 0xB8 249 | #define TW_ST_DATA_NACK 0xC0 250 | #define TW_ST_LAST_DATA 0xC8 251 | #define TW_SR_SLA_ACK 0x60 252 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 253 | #define TW_SR_GCALL_ACK 0x70 254 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 255 | #define TW_SR_DATA_ACK 0x80 256 | #define TW_SR_DATA_NACK 0x88 257 | #define TW_SR_GCALL_DATA_ACK 0x90 258 | #define TW_SR_GCALL_DATA_NACK 0x98 259 | #define TW_SR_STOP 0xA0 260 | #define TW_NO_INFO 0xF8 261 | #define TW_BUS_ERROR 0x00 262 | 263 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 264 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 265 | 266 | #ifndef sbi // set bit 267 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 268 | #endif // sbi 269 | 270 | #ifndef cbi // clear bit 271 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 272 | #endif // cbi 273 | 274 | extern TwoWire Wire; 275 | 276 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 277 | 278 | #endif /* _I2CDEV_H_ */ 279 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/I2Cdev/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For I2Cdev 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | I2Cdev KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | 14 | readBit KEYWORD2 15 | readBitW KEYWORD2 16 | readBits KEYWORD2 17 | readBitsW KEYWORD2 18 | readByte KEYWORD2 19 | readBytes KEYWORD2 20 | readWord KEYWORD2 21 | readWords KEYWORD2 22 | writeBit KEYWORD2 23 | writeBitW KEYWORD2 24 | writeBits KEYWORD2 25 | writeBitsW KEYWORD2 26 | writeByte KEYWORD2 27 | writeBytes KEYWORD2 28 | writeWord KEYWORD2 29 | writeWords KEYWORD2 30 | 31 | ####################################### 32 | # Instances (KEYWORD2) 33 | ####################################### 34 | 35 | ####################################### 36 | # Constants (LITERAL1) 37 | ####################################### 38 | 39 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/I2Cdev/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-Core", 3 | "keywords": "i2cdevlib, i2c", 4 | "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.", 5 | "include": "Arduino/I2Cdev", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "frameworks": "arduino", 12 | "platforms": "atmelavr" 13 | } 14 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050/MPU6050_6Axis_MotionApps20.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 34 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifdef __AVR__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 90 | * unavailable, unless you happen to be amazing as decompiling binary by 91 | * hand (in which case, please contact me, and I'm totally serious). 92 | * 93 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 94 | * DMP reverse-engineering he did to help make this bit of wizardry 95 | * possible. 96 | */ 97 | 98 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 99 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 100 | // after moving string constants to flash memory storage using the F() 101 | // compiler macro (Arduino IDE 1.0+ required). 102 | 103 | //#define DEBUG 104 | #ifdef DEBUG 105 | #define DEBUG_PRINT(x) Serial.print(x) 106 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 107 | #define DEBUG_PRINTLN(x) Serial.println(x) 108 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 109 | #else 110 | #define DEBUG_PRINT(x) 111 | #define DEBUG_PRINTF(x, y) 112 | #define DEBUG_PRINTLN(x) 113 | #define DEBUG_PRINTLNF(x, y) 114 | #endif 115 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 119 | 120 | /* ================================================================================================ * 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 122 | | | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 125 | | | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 297 | // SPECIAL 0x01 = enable interrupts 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 321 | 0x01, 0x62, 0x02, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 323 | }; 324 | 325 | uint8_t MPU6050::dmpInitialize() { 326 | // reset device 327 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 328 | reset(); 329 | delay(30); // wait after reset 330 | 331 | // enable sleep mode and wake cycle 332 | /*Serial.println(F("Enabling sleep mode...")); 333 | setSleepEnabled(true); 334 | Serial.println(F("Enabling wake cycle...")); 335 | setWakeCycleEnabled(true);*/ 336 | 337 | // disable sleep mode 338 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 339 | setSleepEnabled(false); 340 | 341 | // get MPU hardware revision 342 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 343 | setMemoryBank(0x10, true, true); 344 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 345 | setMemoryStartAddress(0x06); 346 | DEBUG_PRINTLN(F("Checking hardware revision...")); 347 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 348 | DEBUG_PRINTLNF(readMemoryByte(), HEX); 349 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 350 | setMemoryBank(0, false, false); 351 | 352 | // check OTP bank valid 353 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 354 | DEBUG_PRINT(F("OTP bank is ")); 355 | DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); 356 | 357 | // get X/Y/Z gyro offsets 358 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 359 | int8_t xgOffsetTC = getXGyroOffsetTC(); 360 | int8_t ygOffsetTC = getYGyroOffsetTC(); 361 | int8_t zgOffsetTC = getZGyroOffsetTC(); 362 | DEBUG_PRINT(F("X gyro offset = ")); 363 | DEBUG_PRINTLN(xgOffsetTC); 364 | DEBUG_PRINT(F("Y gyro offset = ")); 365 | DEBUG_PRINTLN(ygOffsetTC); 366 | DEBUG_PRINT(F("Z gyro offset = ")); 367 | DEBUG_PRINTLN(zgOffsetTC); 368 | 369 | // setup weird slave stuff (?) 370 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 371 | setSlaveAddress(0, 0x7F); 372 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 373 | setI2CMasterModeEnabled(false); 374 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 375 | setSlaveAddress(0, 0x68); 376 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 377 | resetI2CMaster(); 378 | delay(20); 379 | 380 | // load DMP code into memory banks 381 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 382 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 383 | DEBUG_PRINTLN(F(" bytes)")); 384 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 385 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 386 | 387 | // write DMP configuration 388 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 389 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 390 | DEBUG_PRINTLN(F(" bytes in config def)")); 391 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 392 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 393 | 394 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 395 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 396 | 397 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 398 | setIntEnabled(0x12); 399 | 400 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 401 | setRate(4); // 1khz / (1 + 4) = 200 Hz 402 | 403 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 404 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 405 | 406 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 407 | setDLPFMode(MPU6050_DLPF_BW_42); 408 | 409 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 410 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 411 | 412 | DEBUG_PRINTLN(F("Setting DMP programm start address")); 413 | //write start address MSB into register 414 | setDMPConfig1(0x03); 415 | //write start address LSB into register 416 | setDMPConfig2(0x00); 417 | 418 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 419 | setOTPBankValid(false); 420 | 421 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 422 | setXGyroOffsetTC(xgOffsetTC); 423 | setYGyroOffsetTC(ygOffsetTC); 424 | setZGyroOffsetTC(zgOffsetTC); 425 | 426 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 427 | //setXGyroOffset(0); 428 | //setYGyroOffset(0); 429 | //setZGyroOffset(0); 430 | 431 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 432 | uint8_t dmpUpdate[16], j; 433 | uint16_t pos = 0; 434 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 435 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 436 | 437 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 438 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 439 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 440 | 441 | DEBUG_PRINTLN(F("Resetting FIFO...")); 442 | resetFIFO(); 443 | 444 | DEBUG_PRINTLN(F("Reading FIFO count...")); 445 | uint16_t fifoCount = getFIFOCount(); 446 | uint8_t fifoBuffer[128]; 447 | 448 | DEBUG_PRINT(F("Current FIFO count=")); 449 | DEBUG_PRINTLN(fifoCount); 450 | getFIFOBytes(fifoBuffer, fifoCount); 451 | 452 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 453 | setMotionDetectionThreshold(2); 454 | 455 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 456 | setZeroMotionDetectionThreshold(156); 457 | 458 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 459 | setMotionDetectionDuration(80); 460 | 461 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 462 | setZeroMotionDetectionDuration(0); 463 | 464 | DEBUG_PRINTLN(F("Resetting FIFO...")); 465 | resetFIFO(); 466 | 467 | DEBUG_PRINTLN(F("Enabling FIFO...")); 468 | setFIFOEnabled(true); 469 | 470 | DEBUG_PRINTLN(F("Enabling DMP...")); 471 | setDMPEnabled(true); 472 | 473 | DEBUG_PRINTLN(F("Resetting DMP...")); 474 | resetDMP(); 475 | 476 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 477 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 478 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 479 | 480 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 481 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 482 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 483 | 484 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 485 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 486 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 487 | 488 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 489 | while ((fifoCount = getFIFOCount()) < 3); 490 | 491 | DEBUG_PRINT(F("Current FIFO count=")); 492 | DEBUG_PRINTLN(fifoCount); 493 | DEBUG_PRINTLN(F("Reading FIFO data...")); 494 | getFIFOBytes(fifoBuffer, fifoCount); 495 | 496 | DEBUG_PRINTLN(F("Reading interrupt status...")); 497 | 498 | DEBUG_PRINT(F("Current interrupt status=")); 499 | DEBUG_PRINTLNF(getIntStatus(), HEX); 500 | 501 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 502 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 503 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 504 | 505 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 506 | while ((fifoCount = getFIFOCount()) < 3); 507 | 508 | DEBUG_PRINT(F("Current FIFO count=")); 509 | DEBUG_PRINTLN(fifoCount); 510 | 511 | DEBUG_PRINTLN(F("Reading FIFO data...")); 512 | getFIFOBytes(fifoBuffer, fifoCount); 513 | 514 | DEBUG_PRINTLN(F("Reading interrupt status...")); 515 | 516 | DEBUG_PRINT(F("Current interrupt status=")); 517 | DEBUG_PRINTLNF(getIntStatus(), HEX); 518 | 519 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 520 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 521 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 522 | 523 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 524 | 525 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 526 | setDMPEnabled(false); 527 | 528 | DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 529 | dmpPacketSize = 42; 530 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 531 | return 3; // TODO: proper error code for no memory 532 | }*/ 533 | 534 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 535 | resetFIFO(); 536 | getIntStatus(); 537 | } else { 538 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 539 | return 2; // configuration block loading failed 540 | } 541 | } else { 542 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 543 | return 1; // main binary block loading failed 544 | } 545 | return 0; // success 546 | } 547 | 548 | bool MPU6050::dmpPacketAvailable() { 549 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 550 | } 551 | 552 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 553 | // uint8_t MPU6050::dmpGetFIFORate(); 554 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 555 | // uint8_t MPU6050::dmpGetSampleFrequency(); 556 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 557 | 558 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 559 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 560 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 561 | 562 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 563 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 564 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 572 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 573 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 574 | 575 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 576 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 577 | if (packet == 0) packet = dmpPacketBuffer; 578 | data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); 579 | data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); 580 | data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); 581 | return 0; 582 | } 583 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 584 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 585 | if (packet == 0) packet = dmpPacketBuffer; 586 | data[0] = (packet[28] << 8) | packet[29]; 587 | data[1] = (packet[32] << 8) | packet[33]; 588 | data[2] = (packet[36] << 8) | packet[37]; 589 | return 0; 590 | } 591 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 592 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 593 | if (packet == 0) packet = dmpPacketBuffer; 594 | v -> x = (packet[28] << 8) | packet[29]; 595 | v -> y = (packet[32] << 8) | packet[33]; 596 | v -> z = (packet[36] << 8) | packet[37]; 597 | return 0; 598 | } 599 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 600 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 601 | if (packet == 0) packet = dmpPacketBuffer; 602 | data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); 603 | data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); 604 | data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); 605 | data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); 606 | return 0; 607 | } 608 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 609 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 610 | if (packet == 0) packet = dmpPacketBuffer; 611 | data[0] = ((packet[0] << 8) | packet[1]); 612 | data[1] = ((packet[4] << 8) | packet[5]); 613 | data[2] = ((packet[8] << 8) | packet[9]); 614 | data[3] = ((packet[12] << 8) | packet[13]); 615 | return 0; 616 | } 617 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 618 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 619 | int16_t qI[4]; 620 | uint8_t status = dmpGetQuaternion(qI, packet); 621 | if (status == 0) { 622 | q -> w = (float)qI[0] / 16384.0f; 623 | q -> x = (float)qI[1] / 16384.0f; 624 | q -> y = (float)qI[2] / 16384.0f; 625 | q -> z = (float)qI[3] / 16384.0f; 626 | return 0; 627 | } 628 | return status; // int16 return value, indicates error if this line is reached 629 | } 630 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 631 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 632 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 633 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 634 | if (packet == 0) packet = dmpPacketBuffer; 635 | data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); 636 | data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); 637 | data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); 638 | return 0; 639 | } 640 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 641 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 642 | if (packet == 0) packet = dmpPacketBuffer; 643 | data[0] = (packet[16] << 8) | packet[17]; 644 | data[1] = (packet[20] << 8) | packet[21]; 645 | data[2] = (packet[24] << 8) | packet[25]; 646 | return 0; 647 | } 648 | uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { 649 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 650 | if (packet == 0) packet = dmpPacketBuffer; 651 | v -> x = (packet[16] << 8) | packet[17]; 652 | v -> y = (packet[20] << 8) | packet[21]; 653 | v -> z = (packet[24] << 8) | packet[25]; 654 | return 0; 655 | } 656 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 657 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 658 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 659 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 660 | v -> x = vRaw -> x - gravity -> x*8192; 661 | v -> y = vRaw -> y - gravity -> y*8192; 662 | v -> z = vRaw -> z - gravity -> z*8192; 663 | return 0; 664 | } 665 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 666 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 667 | // rotate measured 3D acceleration vector into original state 668 | // frame of reference based on orientation quaternion 669 | memcpy(v, vReal, sizeof(VectorInt16)); 670 | v -> rotate(q); 671 | return 0; 672 | } 673 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 674 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 675 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 676 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 677 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 678 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 679 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 680 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 681 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 682 | return 0; 683 | } 684 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 685 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 686 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 687 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 688 | 689 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 690 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 691 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 692 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 693 | return 0; 694 | } 695 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 696 | // yaw: (about Z axis) 697 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 698 | // pitch: (nose up/down, about Y axis) 699 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 700 | // roll: (tilt left/right, about X axis) 701 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 702 | return 0; 703 | } 704 | 705 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 706 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 707 | 708 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 709 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 710 | if (dmpData[k] < 0x10) Serial.print("0"); 711 | Serial.print(dmpData[k], HEX); 712 | Serial.print(" "); 713 | } 714 | Serial.print("\n");*/ 715 | //Serial.println((uint16_t)dmpPacketBuffer); 716 | return 0; 717 | } 718 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 719 | uint8_t status; 720 | uint8_t buf[dmpPacketSize]; 721 | for (uint8_t i = 0; i < numPackets; i++) { 722 | // read packet from FIFO 723 | getFIFOBytes(buf, dmpPacketSize); 724 | 725 | // process packet 726 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 727 | 728 | // increment external process count variable, if supplied 729 | if (processed != 0) (*processed)++; 730 | } 731 | return 0; 732 | } 733 | 734 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 735 | 736 | // uint8_t MPU6050::dmpInitFIFOParam(); 737 | // uint8_t MPU6050::dmpCloseFIFO(); 738 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 739 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 740 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 741 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 742 | // void MPU6050::dmpOverrideQuaternion(long *q); 743 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 744 | return dmpPacketSize; 745 | } 746 | 747 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 748 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-MPU6050", 3 | "keywords": "gyroscope, accelerometer, sensor, i2cdevlib, i2c", 4 | "description": "The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms", 5 | "include": "Arduino/MPU6050", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "dependencies": 12 | { 13 | "name": "I2Cdevlib-Core", 14 | "frameworks": "arduino" 15 | }, 16 | "frameworks": "arduino", 17 | "platforms": "atmelavr" 18 | } 19 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/DeathTimer.h: -------------------------------------------------------------------------------- 1 | 2 | /* ============================================ 3 | I2Cdev device library code is placed under the MIT license 4 | Copyright (c) 2012, 2016 Jeff Rowberg 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | =============================================== 24 | */ 25 | 26 | #ifndef _DeathTimer_H 27 | #define _DeathTimer_H 28 | 29 | #include 30 | 31 | class DeathTimer { 32 | public: 33 | DeathTimer(uint32_t period): _period(period) { 34 | _startTime = _lastPrintout = millis(); 35 | } 36 | 37 | bool update() { 38 | uint32_t now = millis(); 39 | if ((now - _lastPrintout) > _period) { 40 | _lastPrintout = now; 41 | uint32_t time = (now - _startTime)/1000L; // in seconds 42 | 43 | Serial.print(F("H:")); Serial.print(time / (60*60)); 44 | Serial.print(F("M:")); Serial.print((time / 60) % 60); 45 | Serial.print(F("S:")); Serial.println(time % 60); 46 | return true; 47 | } 48 | return false; 49 | } 50 | 51 | void setPeriod(uint16_t period) { 52 | _period = period; 53 | } 54 | 55 | protected: 56 | uint16_t _period; 57 | uint32_t _lastPrintout; 58 | uint32_t _startTime; 59 | }; 60 | 61 | #endif // DeathTimer_H 62 | 63 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | {one line to give the program's name and a brief idea of what it does.} 635 | Copyright (C) {year} {name of author} 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | {project} Copyright (C) {year} {fullname} 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/MPU6050_Wrapper.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2 | // 2016-05-14 github.com/eadf 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2016-05-14 - First revision 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012, 2016 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | 33 | #ifndef MPU6050_WRAPPER_H 34 | #define MPU6050_WRAPPER_H 35 | 36 | #define MPU6050_DMP_FIFO_RATE_DIVISOR 4 // Set FIFO rate to 40Hz (200/(4+1)) 37 | #define MPU6050_DMP_FIFO_PERIOD ((uint32_t)(1000.0*((MPU6050_DMP_FIFO_RATE_DIVISOR+1.0))/200.0)-2) // hopefully this will be converted to a uint32_t constant by the compiler 38 | 39 | #include "MPU6050_6Axis_MotionApps20.h" 40 | 41 | class MPU6050_Wrapper { 42 | 43 | friend class MPU6050_Array; 44 | public: 45 | MPU6050_Wrapper(uint8_t ad0Pin) : _mpu(MPU6050_ADDRESS_AD0_HIGH), _ad0Pin(ad0Pin) { 46 | pinMode(_ad0Pin, OUTPUT); 47 | digitalWrite(_ad0Pin, LOW); 48 | } 49 | 50 | uint8_t dmpInitialize() { 51 | return _devStatus = _mpu.dmpInitialize(); 52 | } 53 | 54 | uint8_t getIntStatus() { 55 | return _mpuIntStatus = _mpu.getIntStatus(); 56 | } 57 | 58 | uint16_t dmpGetFIFOPacketSize() { 59 | return _packetSize = _mpu.dmpGetFIFOPacketSize(); 60 | } 61 | 62 | uint16_t getFIFOCount() { 63 | //Serial.print(F(" getFIFOCount on _ad0Pin:")); Serial.println(_ad0Pin); 64 | return _fifoCount = _mpu.getFIFOCount(); 65 | } 66 | 67 | uint16_t resetFIFO() { 68 | _mpu.resetFIFO(); 69 | return getFIFOCount(); 70 | } 71 | 72 | void getFIFOBytes(uint8_t* fifoBuffer) { 73 | _mpu.getFIFOBytes(fifoBuffer, _packetSize); 74 | _fifoCount -= _packetSize; 75 | } 76 | 77 | bool isDue() { 78 | if ((millis() - _lastUpdate) > MPU6050_DMP_FIFO_PERIOD) { 79 | if (getFIFOCount() >= _packetSize) return true; 80 | else { 81 | //Serial.print("F"); 82 | } 83 | } 84 | return false; 85 | } 86 | 87 | private: 88 | void enable(bool status) { 89 | //Serial.print(F(" Setting AD0 pin ")); Serial.print(_ad0Pin); Serial.print(F(" to ")); Serial.println( status ? F("on") : F("off")); 90 | digitalWrite(_ad0Pin, status); 91 | } 92 | 93 | public: 94 | MPU6050 _mpu; 95 | uint8_t _ad0Pin; 96 | uint8_t _mpuIntStatus = 0; // holds actual interrupt status byte from MPU 97 | uint8_t _devStatus = 0; // return status after each device operation (0 = success, !0 = error) 98 | uint8_t _packetSize = 0; // expected DMP packet size (default is 42 bytes) 99 | uint16_t _fifoCount = 0; // count of all bytes currently in FIFO 100 | uint32_t _lastUpdate = 0; 101 | }; 102 | 103 | class MPU6050_Array { 104 | public: 105 | MPU6050_Array(uint8_t size): _size(size) { 106 | _array = (MPU6050_Wrapper**) malloc(sizeof(MPU6050_Wrapper*) * _size); 107 | } 108 | 109 | void add( uint8_t ad0Pin) { 110 | if (_fillIndex >= _size) halt(F("MPU6050_Array::add() : overflow")); 111 | _array[_fillIndex++] = new MPU6050_Wrapper(ad0Pin); 112 | } 113 | 114 | MPU6050_Wrapper* select(uint8_t device) { 115 | if (_currentIndex == device) 116 | return getCurrent(); 117 | for (int i = 0; i < _fillIndex; i++) 118 | _array[i]->enable(i == device); 119 | _currentIndex = device; 120 | // give the IMU some time to realize that the AD0 pin is altered 121 | delay(2); 122 | return getCurrent(); 123 | } 124 | 125 | inline MPU6050_Wrapper* getCurrent() { 126 | return _array[_currentIndex]; 127 | } 128 | 129 | void initialize() { 130 | for (int i = 0; i < _fillIndex; i++) { 131 | select(i); 132 | _array[i]->_mpu.initialize(); 133 | } 134 | } 135 | 136 | bool testConnection() { 137 | for (int i = 0; i < _fillIndex; i++) { 138 | select(i); 139 | if (!_array[i]->_mpu.testConnection()) 140 | return false; 141 | } 142 | return true; 143 | } 144 | 145 | void dmpInitialize() { 146 | for (int i = 0; i < _fillIndex; i++) { 147 | select(i); 148 | _array[i]->dmpInitialize(); 149 | } 150 | } 151 | 152 | bool programDmp(uint8_t mpu) { 153 | MPU6050_Wrapper* currentMPU = select(mpu); 154 | if (currentMPU->_devStatus == 0) { 155 | // turn on the DMP, now that it's ready 156 | Serial.println(F("Enabling DMP...")); 157 | currentMPU->_mpu.setDMPEnabled(true); 158 | currentMPU->getIntStatus(); 159 | 160 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 161 | Serial.println(F("DMP ready!")); 162 | 163 | // get expected DMP packet size for later comparison 164 | currentMPU->dmpGetFIFOPacketSize(); 165 | 166 | Serial.print(F("The DMP sample period is ")); Serial.print(MPU6050_DMP_FIFO_PERIOD); Serial.println(F(" ms")); 167 | currentMPU->_lastUpdate = millis(); 168 | 169 | if (currentMPU->_devStatus) { 170 | // ERROR! 171 | // 1 = initial memory load failed 172 | // 2 = DMP configuration updates failed 173 | // (if it's going to break, usually the code will be 1) 174 | Serial.print(F("DMP: ")); Serial.print(mpu); 175 | Serial.print(F(" Initialization failed (code ")); 176 | Serial.print(currentMPU->_devStatus); 177 | halt(F(") Halting..")); 178 | } else { 179 | return currentMPU->_devStatus; 180 | } 181 | } else { 182 | Serial.print(F("MPU6050 error code ")); 183 | Serial.print(currentMPU->_devStatus); 184 | halt(F(" Halting..")); 185 | } 186 | } 187 | 188 | void halt(const __FlashStringHelper* errMessage = NULL) { 189 | if (errMessage) 190 | Serial.println(errMessage); 191 | while (true) 192 | ; 193 | } 194 | 195 | private: 196 | MPU6050_Wrapper** _array = NULL; 197 | uint8_t _fillIndex = 0; // where add() will place next mpu* 198 | int8_t _currentIndex = -1; 199 | int8_t _size = -1; 200 | }; 201 | 202 | #endif 203 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/README.md: -------------------------------------------------------------------------------- 1 | # MPU6050_DMP6_Multiple 2 | I will move this to https://github.com/jrowberg/i2cdevlib once this example is tested. 3 | 4 | This example connects to two MPU6050. But it is possible to extend the example to connect to any number of accelerometers. Only the number of output GPIO pins sets the limit. 5 | 6 | In the unaltered example you need to connect your MPU6050 AD0 pins to Arduino like this: 7 | ``` 8 | #define AD0_PIN_0 4 // Connect this pin to the AD0 pin on IMU #0 9 | #define AD0_PIN_1 5 // Connect this pin to the AD0 pin on IMU #1 10 | ``` 11 | Note that the INT pin of the MPU6050 isn't used anymore. 12 | 13 | To use this example you *must* checkout the develop branch of i2cdevlib: 14 | ``` 15 | cd i2cdevlib 16 | git checkout develop 17 | ``` 18 | Make sure that the Arduino library manager uses the develop branch. 19 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/MPU6050_DMP6_Multiple-master/TogglePin.h: -------------------------------------------------------------------------------- 1 | 2 | /* ============================================ 3 | I2Cdev device library code is placed under the MIT license 4 | Copyright (c) 2012, 2016 Jeff Rowberg 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | =============================================== 24 | */ 25 | 26 | #ifndef TOGGLEPIN_H 27 | #define TOGGLEPIN_H 28 | 29 | #include 30 | 31 | class TogglePin { 32 | public: 33 | TogglePin(uint8_t pin, uint32_t period):_pin(pin), _period(period) { 34 | _lastToggle = millis(); 35 | _state = false; 36 | pinMode(_pin, OUTPUT); 37 | digitalWrite(_pin, _state); 38 | } 39 | 40 | bool update() { 41 | uint32_t now = millis(); 42 | if ((now - _lastToggle) > _period) { 43 | digitalWrite(_pin, _state = !_state); 44 | _lastToggle = now; 45 | return true; 46 | } 47 | return false; 48 | } 49 | 50 | void setPeriod(uint16_t period) { 51 | _period = period; 52 | } 53 | 54 | protected: 55 | const uint8_t _pin; 56 | bool _state; 57 | uint16_t _period; 58 | uint32_t _lastToggle; 59 | }; 60 | 61 | #endif // TOGGLEPIN_H 62 | 63 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Arduino/libraries/Readme: -------------------------------------------------------------------------------- 1 | Copy the folders in this directory and paste it C:\Users\"UserName"\Documents\Arduino folder of your computer 2 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Processing/Soleoffline/Plantar Pressure Distribution 2.csv: -------------------------------------------------------------------------------- 1 | timestamp millis,Plantar Pressure Voltage 2 | 26110,"0.03,0.00,0.03,0.00,0.01,0.01,0.02,0.00,0.03,0.04,0.02,0.01,0.00,0.10,0.05,0.04,0.00,0.00,0.16,0.05,0.04,0.05,0.01,0.13,0.04,0.05,0.09,0.03,0.06,0.02,0.02,0.06,0.06,0.05,0.03,0.02,0.06,0.05,0.01,0.02,0.07,0.04,0.02,0.02,0.13,0.02,0.02,0.02,0.07,0.06,0.07,0.00,0.03,0.12,0.08,0.00,0.03,0.10,0.02,0.00,0.09,0.15,0.02,0.01" 3 | 26178,"0.11,0.04,0.06,0.01,0.07,0.01,0.02,0.00,0.04,0.03,0.01,0.00,0.00,0.12,0.08,0.05,0.03,0.01,0.20,0.04,0.08,0.11,0.02,0.12,0.03,0.07,0.12,0.06,0.07,0.01,0.03,0.08,0.09,0.02,0.01,0.02,0.08,0.03,0.03,0.03,0.09,0.04,0.03,0.07,0.20,0.05,0.06,0.07,0.09,0.15,0.15,0.10,0.05,0.31,0.24,0.11,0.03,0.29,0.22,0.12,0.11,0.04,0.08,0.96" 4 | 26244,"0.02,0.02,0.01,0.00,0.01,0.01,0.01,0.00,0.04,0.02,0.05,0.02,0.00,0.08,0.02,0.02,0.02,0.01,0.10,0.03,0.03,0.03,0.03,0.11,0.03,0.03,0.04,0.03,0.07,0.03,0.02,0.03,0.06,0.04,0.01,0.02,0.08,0.03,0.02,0.03,0.11,0.06,0.03,0.06,0.23,0.10,0.11,0.10,0.14,0.29,0.26,0.29,0.11,0.40,0.37,0.38,0.13,0.34,0.38,0.47,0.9,0.95,0.6,0.89" 5 | 26310,"0.01,0.01,0.02,0.00,0.01,0.01,0.01,0.00,0.05,0.03,0.01,0.02,0.00,0.08,0.03,0.00,0.02,0.03,0.11,0.03,0.04,0.05,0.05,0.08,0.03,0.07,0.08,0.11,0.05,0.02,0.05,0.05,0.11,0.02,0.03,0.05,0.13,0.04,0.07,0.08,0.22,0.07,0.13,0.16,0.42,0.14,0.23,0.24,0.31,0.35,0.32,0.30,0.27,0.46,0.38,0.32,0.29,0.53,0.35,0.47,0.8,0.65,0.78,1.01" 6 | 26378,"0.11,0.05,0.04,0.01,0.04,0.01,0.00,0.00,0.01,0.03,0.03,0.03,0.00,0.07,0.05,0.06,0.06,0.03,0.06,0.03,0.10,0.15,0.13,0.05,0.04,0.13,0.22,0.19,0.03,0.03,0.11,0.13,0.22,0.04,0.06,0.08,0.20,0.05,0.10,0.13,0.26,0.10,0.15,0.24,0.44,0.17,0.29,0.28,0.30,0.43,0.35,0.34,0.25,0.49,0.37,0.38,0.27,0.54,0.45,0.51,1.05,0.59,0.92,0.96" 7 | 26444,"0.16,0.11,0.09,0.01,0.08,0.04,0.05,0.01,0.03,0.04,0.03,0.04,0.01,0.14,0.13,0.21,0.15,0.04,0.22,0.11,0.25,0.23,0.16,0.13,0.05,0.23,0.28,0.23,0.05,0.01,0.06,0.22,0.31,0.03,0.04,0.16,0.28,0.04,0.06,0.19,0.33,0.06,0.19,0.28,0.46,0.19,0.32,0.30,0.30,0.42,0.35,0.31,0.18,0.50,0.40,0.32,0.16,0.53,0.41,0.27,0.14,0.34,0.55,0.24" 8 | 26510,"0.20,0.16,0.13,0.02,0.13,0.07,0.17,0.02,0.08,0.08,0.09,0.09,0.01,0.23,0.23,0.32,0.19,0.06,0.32,0.15,0.31,0.27,0.18,0.19,0.10,0.28,0.30,0.23,0.07,0.02,0.10,0.27,0.36,0.03,0.02,0.20,0.35,0.05,0.08,0.23,0.39,0.06,0.19,0.29,0.47,0.18,0.31,0.28,0.29,0.39,0.33,0.29,0.20,0.48,0.37,0.26,0.12,0.47,0.39,0.22,0.10,0.25,0.44,0.17" 9 | 26577,"0.25,0.20,0.12,0.05,0.14,0.08,0.22,0.01,0.10,0.10,0.10,0.13,0.03,0.28,0.25,0.35,0.21,0.06,0.36,0.19,0.35,0.25,0.21,0.20,0.11,0.31,0.31,0.25,0.05,0.04,0.10,0.30,0.40,0.00,0.04,0.26,0.39,0.03,0.07,0.27,0.40,0.06,0.20,0.29,0.48,0.18,0.29,0.29,0.31,0.41,0.31,0.29,0.17,0.47,0.38,0.27,0.09,0.45,0.34,0.16,0.09,0.23,0.36,0.12" 10 | 26642,"0.29,0.21,0.09,0.03,0.13,0.10,0.25,0.02,0.13,0.11,0.17,0.17,0.02,0.31,0.27,0.37,0.23,0.06,0.38,0.18,0.33,0.27,0.23,0.20,0.13,0.34,0.33,0.27,0.07,0.05,0.11,0.34,0.42,0.02,0.05,0.26,0.39,0.02,0.10,0.29,0.39,0.06,0.20,0.28,0.47,0.18,0.29,0.27,0.24,0.35,0.28,0.27,0.11,0.44,0.35,0.18,0.04,0.36,0.24,0.09,0.06,0.16,0.16,0.09" 11 | 26710,"0.34,0.24,0.12,0.06,0.21,0.11,0.20,0.00,0.23,0.17,0.25,0.18,0.02,0.37,0.30,0.39,0.24,0.06,0.47,0.22,0.36,0.26,0.19,0.26,0.16,0.37,0.33,0.26,0.07,0.02,0.12,0.35,0.42,0.03,0.05,0.28,0.37,0.03,0.07,0.23,0.34,0.05,0.18,0.24,0.43,0.15,0.22,0.24,0.19,0.32,0.22,0.14,0.04,0.34,0.24,0.07,0.03,0.18,0.11,0.03,0.06,0.12,0.04,0.03" 12 | 26778,"0.40,0.25,0.11,0.09,0.27,0.15,0.18,0.00,0.33,0.25,0.32,0.19,0.02,0.43,0.36,0.40,0.24,0.05,0.54,0.27,0.37,0.25,0.16,0.31,0.20,0.39,0.32,0.23,0.08,0.02,0.12,0.34,0.37,0.03,0.04,0.21,0.25,0.03,0.04,0.17,0.22,0.05,0.10,0.22,0.33,0.10,0.14,0.14,0.12,0.22,0.15,0.04,0.02,0.22,0.15,0.01,0.03,0.11,0.02,0.00,0.06,0.11,0.01,0.01" 13 | 26842,"0.43,0.23,0.08,0.13,0.32,0.19,0.20,0.01,0.43,0.31,0.35,0.18,0.02,0.49,0.40,0.39,0.23,0.06,0.59,0.28,0.37,0.23,0.13,0.28,0.19,0.35,0.31,0.17,0.05,0.04,0.07,0.20,0.26,0.05,0.02,0.11,0.14,0.02,0.02,0.10,0.12,0.04,0.05,0.12,0.21,0.09,0.10,0.06,0.07,0.17,0.13,0.00,0.03,0.17,0.09,0.00,0.01,0.10,0.02,0.00,0.05,0.12,0.10,0.07" 14 | 26909,"0.43,0.22,0.09,0.13,0.36,0.22,0.20,0.01,0.43,0.35,0.33,0.16,0.00,0.53,0.41,0.39,0.20,0.03,0.60,0.30,0.35,0.21,0.08,0.32,0.19,0.25,0.19,0.10,0.09,0.03,0.05,0.13,0.20,0.03,0.01,0.04,0.14,0.03,0.03,0.03,0.10,0.04,0.01,0.05,0.16,0.03,0.01,0.00,0.06,0.04,0.01,0.03,0.04,0.05,0.01,0.00,0.04,0.10,0.00,0.00,0.06,0.09,0.06,0.02" 15 | 26976,"0.36,0.16,0.06,0.63,0.18,0.05,0.11,0.08,0.38,0.16,0.09,0.06,0.18,0.46,0.30,0.24,0.13,0.07,0.53,0.25,0.26,0.13,0.06,0.46,0.21,0.18,0.09,0.05,0.08,0.04,0.05,0.06,0.06,0.01,0.00,0.03,0.08,0.03,0.00,0.01,0.05,0.02,0.01,0.00,0.08,0.02,0.00,0.00,0.06,0.02,0.01,0.01,0.02,0.03,0.00,0.00,0.04,0.09,0.00,0.00,0.07,0.10,0.00,0.00" 16 | 27041,"0.07,0.03,0.03,0.02,0.07,0.02,0.02,0.00,0.04,0.05,0.05,0.02,0.00,0.16,0.12,0.08,0.03,0.00,0.25,0.06,0.10,0.06,0.02,0.12,0.02,0.05,0.09,0.06,0.07,0.02,0.02,0.04,0.05,0.03,0.03,0.02,0.05,0.03,0.01,0.01,0.05,0.02,0.01,0.00,0.10,0.00,0.02,0.03,0.08,0.08,0.06,0.01,0.04,0.22,0.11,0.00,0.03,0.16,0.05,0.00,0.07,0.15,0.02,0.00" 17 | 27109,"0.03,0.02,0.02,0.02,0.02,0.01,0.01,0.00,0.04,0.05,0.03,0.00,0.00,0.18,0.08,0.06,0.01,0.00,0.33,0.06,0.08,0.05,0.01,0.21,0.02,0.06,0.09,0.04,0.08,0.02,0.02,0.04,0.04,0.03,0.03,0.02,0.05,0.04,0.01,0.01,0.05,0.03,0.01,0.00,0.10,0.02,0.02,0.02,0.08,0.09,0.07,0.01,0.04,0.21,0.12,0.00,0.03,0.15,0.05,0.00,0.09,0.17,0.02,0.01" 18 | 27173,"0.04,0.03,0.01,0.02,0.03,0.00,0.01,0.02,0.06,0.04,0.02,0.00,0.00,0.18,0.09,0.08,0.01,0.01,0.31,0.06,0.09,0.04,0.01,0.20,0.04,0.07,0.08,0.02,0.10,0.02,0.03,0.06,0.04,0.04,0.00,0.01,0.04,0.04,0.01,0.01,0.05,0.03,0.00,0.00,0.12,0.04,0.02,0.00,0.06,0.12,0.09,0.00,0.04,0.22,0.13,0.00,0.02,0.18,0.08,0.01,0.08,0.22,0.02,0.00" 19 | 27241,"0.04,0.03,0.02,0.02,0.02,0.00,0.01,0.01,0.04,0.03,0.00,0.01,0.00,0.16,0.09,0.09,0.03,0.00,0.30,0.07,0.08,0.05,0.00,0.20,0.04,0.08,0.09,0.03,0.09,0.03,0.04,0.05,0.05,0.04,0.00,0.02,0.05,0.04,0.01,0.01,0.04,0.02,0.01,0.03,0.12,0.04,0.02,0.00,0.08,0.10,0.08,0.03,0.04,0.23,0.15,0.01,0.04,0.18,0.09,0.00,0.06,0.20,0.02,0.02" 20 | 27308,"0.07,0.03,0.03,0.02,0.04,0.00,0.00,0.00,0.04,0.03,0.01,0.00,0.03,0.12,0.06,0.06,0.04,0.01,0.24,0.06,0.07,0.05,0.00,0.18,0.04,0.06,0.10,0.04,0.08,0.02,0.01,0.04,0.07,0.04,0.02,0.03,0.05,0.04,0.01,0.01,0.05,0.02,0.01,0.03,0.14,0.04,0.03,0.00,0.08,0.09,0.07,0.00,0.04,0.19,0.09,0.00,0.06,0.15,0.06,0.00,0.09,0.17,0.01,0.01" 21 | 27373,"0.02,0.01,0.04,0.00,0.04,0.03,0.02,0.00,0.04,0.04,0.02,0.01,0.00,0.14,0.08,0.06,0.00,0.00,0.23,0.09,0.08,0.06,0.01,0.16,0.02,0.07,0.10,0.04,0.08,0.03,0.03,0.05,0.05,0.04,0.01,0.03,0.04,0.06,0.02,0.01,0.07,0.04,0.01,0.00,0.12,0.04,0.02,0.00,0.08,0.11,0.07,0.00,0.05,0.18,0.12,0.00,0.04,0.12,0.04,0.00,0.09,0.17,0.02,0.03" 22 | 27441,"0.04,0.02,0.03,0.01,0.04,0.01,0.00,0.00,0.07,0.04,0.03,0.00,0.00,0.13,0.06,0.05,0.01,0.00,0.23,0.04,0.07,0.07,0.03,0.15,0.04,0.09,0.12,0.04,0.09,0.04,0.02,0.06,0.06,0.03,0.01,0.02,0.05,0.02,0.02,0.03,0.06,0.02,0.02,0.02,0.15,0.03,0.02,0.01,0.08,0.06,0.05,0.00,0.06,0.14,0.08,0.00,0.04,0.09,0.03,0.00,0.08,0.13,0.01,0.00" 23 | 27507,"0.05,0.01,0.03,0.02,0.05,0.00,0.01,0.00,0.03,0.04,0.02,0.01,0.02,0.14,0.07,0.06,0.02,0.00,0.19,0.05,0.08,0.08,0.04,0.12,0.03,0.07,0.12,0.06,0.06,0.02,0.03,0.07,0.07,0.03,0.01,0.04,0.10,0.01,0.01,0.04,0.09,0.03,0.02,0.06,0.19,0.04,0.05,0.05,0.09,0.15,0.12,0.05,0.06,0.31,0.21,0.06,0.03,0.26,0.18,0.07,0.06,0.18,0.22,0.7" 24 | 27573,"0.01,0.01,0.00,0.01,0.02,0.01,0.00,0.00,0.05,0.03,0.01,0.01,0.00,0.07,0.04,0.03,0.03,0.04,0.13,0.04,0.04,0.02,0.01,0.12,0.02,0.02,0.07,0.07,0.07,0.02,0.02,0.04,0.08,0.02,0.04,0.02,0.09,0.02,0.05,0.03,0.13,0.05,0.06,0.07,0.25,0.09,0.12,0.14,0.16,0.26,0.27,0.29,0.13,0.38,0.40,0.39,0.13,0.17,0.16,0.18,0.8,0.74,0.73,0.74" 25 | 27640,"0.09,0.02,0.02,0.00,0.03,0.01,0.01,0.00,0.03,0.06,0.02,0.01,0.01,0.06,0.03,0.02,0.00,0.01,0.05,0.04,0.07,0.07,0.06,0.03,0.02,0.11,0.13,0.16,0.03,0.02,0.07,0.07,0.16,0.05,0.06,0.05,0.17,0.04,0.08,0.09,0.23,0.09,0.15,0.21,0.39,0.21,0.28,0.26,0.29,0.44,0.33,0.28,0.24,0.50,0.39,0.33,0.25,0.21,0.19,0.21,0.78,0.17,0.24,0.67" 26 | 27705,"0.23,0.09,0.09,0.02,0.08,0.03,0.03,0.00,0.10,0.01,0.02,0.05,0.01,0.22,0.14,0.15,0.10,0.03,0.30,0.10,0.20,0.19,0.10,0.18,0.08,0.16,0.22,0.17,0.07,0.02,0.06,0.16,0.21,0.02,0.05,0.14,0.18,0.06,0.06,0.14,0.25,0.11,0.19,0.24,0.39,0.25,0.31,0.27,0.25,0.51,0.39,0.27,0.18,0.57,0.44,0.34,0.15,0.57,0.43,0.31,0.15,0.48,0.55,0.29" 27 | 27773,"0.22,0.15,0.13,0.04,0.13,0.04,0.09,0.01,0.17,0.07,0.06,0.02,0.00,0.33,0.23,0.24,0.12,0.03,0.43,0.16,0.26,0.20,0.09,0.27,0.11,0.22,0.23,0.14,0.11,0.04,0.08,0.18,0.20,0.06,0.03,0.12,0.19,0.07,0.08,0.18,0.26,0.08,0.17,0.23,0.39,0.27,0.32,0.24,0.22,0.45,0.36,0.29,0.16,0.56,0.40,0.30,0.12,0.55,0.41,0.26,0.09,0.36,0.52,0.22" 28 | 27841,"0.29,0.21,0.10,0.07,0.16,0.06,0.16,0.01,0.25,0.11,0.10,0.10,0.03,0.40,0.29,0.31,0.16,0.04,0.49,0.21,0.30,0.20,0.12,0.32,0.15,0.26,0.23,0.17,0.10,0.04,0.10,0.21,0.24,0.02,0.04,0.16,0.24,0.04,0.09,0.20,0.28,0.09,0.20,0.23,0.39,0.28,0.33,0.25,0.22,0.43,0.35,0.26,0.11,0.53,0.39,0.25,0.07,0.54,0.36,0.16,0.07,0.28,0.43,0.13" 29 | 27906,"0.32,0.24,0.11,0.09,0.19,0.09,0.18,0.01,0.33,0.17,0.19,0.14,0.02,0.46,0.35,0.33,0.18,0.03,0.58,0.25,0.31,0.20,0.13,0.39,0.19,0.30,0.24,0.16,0.12,0.07,0.13,0.21,0.27,0.03,0.05,0.17,0.23,0.03,0.09,0.21,0.27,0.11,0.20,0.22,0.36,0.30,0.27,0.22,0.16,0.40,0.30,0.22,0.07,0.50,0.36,0.15,0.03,0.43,0.27,0.08,0.06,0.21,0.18,0.08" 30 | 27973,"0.35,0.22,0.11,0.13,0.25,0.13,0.19,0.00,0.43,0.24,0.25,0.14,0.01,0.51,0.36,0.34,0.19,0.05,0.62,0.28,0.34,0.21,0.12,0.44,0.24,0.31,0.24,0.15,0.15,0.06,0.12,0.22,0.26,0.05,0.05,0.18,0.19,0.05,0.08,0.18,0.18,0.09,0.17,0.21,0.27,0.24,0.23,0.17,0.11,0.39,0.23,0.11,0.02,0.39,0.23,0.06,0.03,0.25,0.12,0.01,0.05,0.14,0.05,0.04" 31 | 28039,"0.39,0.24,0.11,0.15,0.29,0.15,0.16,0.00,0.44,0.30,0.29,0.15,0.01,0.52,0.39,0.38,0.20,0.02,0.65,0.30,0.35,0.21,0.08,0.46,0.24,0.35,0.24,0.13,0.14,0.04,0.11,0.22,0.20,0.04,0.03,0.13,0.14,0.04,0.04,0.10,0.13,0.08,0.13,0.14,0.23,0.18,0.17,0.09,0.08,0.30,0.15,0.03,0.00,0.29,0.17,0.03,0.01,0.12,0.05,0.01,0.07,0.11,0.01,0.01" 32 | 28105,"0.39,0.23,0.13,0.17,0.31,0.19,0.16,0.01,0.48,0.33,0.31,0.14,0.00,0.55,0.42,0.38,0.19,0.03,0.66,0.29,0.35,0.22,0.09,0.42,0.23,0.32,0.22,0.10,0.11,0.04,0.08,0.15,0.16,0.04,0.02,0.06,0.12,0.05,0.04,0.08,0.12,0.06,0.06,0.12,0.20,0.14,0.11,0.04,0.04,0.26,0.13,0.03,0.01,0.25,0.12,0.01,0.02,0.11,0.02,0.00,0.05,0.15,0.11,0.04" 33 | 28173,"0.40,0.24,0.12,0.19,0.35,0.19,0.18,0.00,0.49,0.38,0.31,0.12,0.01,0.58,0.43,0.37,0.18,0.03,0.67,0.33,0.33,0.19,0.07,0.38,0.21,0.23,0.16,0.09,0.08,0.03,0.05,0.11,0.15,0.04,0.02,0.05,0.13,0.06,0.01,0.04,0.08,0.04,0.02,0.08,0.18,0.07,0.05,0.00,0.07,0.14,0.09,0.01,0.02,0.15,0.06,0.00,0.03,0.13,0.02,0.00,0.04,0.12,0.08,0.04" 34 | 28238,"0.42,0.23,0.21,0.16,0.32,0.18,0.16,0.09,0.46,0.30,0.25,0.09,0.11,0.56,0.40,0.34,0.16,0.06,0.62,0.32,0.28,0.14,0.08,0.40,0.21,0.22,0.12,0.08,0.08,0.04,0.05,0.08,0.13,0.01,0.01,0.05,0.12,0.03,0.00,0.03,0.08,0.02,0.00,0.01,0.13,0.03,0.00,0.00,0.08,0.04,0.02,0.01,0.04,0.06,0.00,0.00,0.04,0.11,0.00,0.00,0.05,0.09,0.03,0.12" 35 | 28305,"0.32,0.11,0.07,0.69,0.14,0.04,0.06,0.04,0.35,0.13,0.08,0.04,0.10,0.48,0.29,0.21,0.06,0.05,0.55,0.24,0.22,0.09,0.05,0.41,0.19,0.15,0.06,0.05,0.09,0.03,0.03,0.02,0.04,0.01,0.02,0.02,0.05,0.01,0.00,0.00,0.03,0.02,0.00,0.00,0.07,0.05,0.01,0.00,0.04,0.03,0.00,0.00,0.02,0.04,0.00,0.00,0.02,0.08,0.00,0.01,0.06,0.13,0.02,0.00" 36 | 28372,"0.02,0.02,0.03,0.01,0.03,0.01,0.01,0.00,0.07,0.02,0.03,0.01,0.00,0.13,0.05,0.06,0.02,0.01,0.21,0.06,0.08,0.03,0.01,0.17,0.03,0.05,0.05,0.03,0.08,0.02,0.03,0.06,0.05,0.02,0.02,0.02,0.04,0.03,0.01,0.01,0.05,0.06,0.02,0.00,0.11,0.05,0.02,0.00,0.06,0.07,0.05,0.00,0.04,0.20,0.10,0.00,0.03,0.14,0.08,0.00,0.06,0.13,0.01,0.01" 37 | 28437,"0.02,0.01,0.02,0.00,0.02,0.00,0.00,0.00,0.05,0.01,0.02,0.01,0.00,0.18,0.08,0.06,0.02,0.01,0.34,0.09,0.08,0.04,0.00,0.21,0.07,0.06,0.06,0.03,0.10,0.02,0.01,0.02,0.07,0.01,0.01,0.04,0.06,0.04,0.00,0.01,0.06,0.03,0.01,0.00,0.12,0.04,0.00,0.00,0.09,0.11,0.09,0.00,0.05,0.22,0.12,0.00,0.03,0.15,0.08,0.00,0.06,0.16,0.02,0.03" 38 | 28505,"0.02,0.00,0.03,0.00,0.02,0.03,0.03,0.00,0.06,0.03,0.02,0.01,0.00,0.23,0.10,0.05,0.03,0.00,0.37,0.08,0.06,0.04,0.03,0.23,0.05,0.07,0.07,0.04,0.08,0.03,0.04,0.02,0.05,0.04,0.01,0.02,0.04,0.02,0.00,0.03,0.07,0.04,0.00,0.01,0.13,0.04,0.02,0.00,0.08,0.10,0.08,0.00,0.05,0.23,0.13,0.00,0.04,0.17,0.09,0.00,0.08,0.19,0.02,0.00" 39 | 28572,"0.02,0.02,0.03,0.01,0.03,0.01,0.01,0.00,0.06,0.03,0.02,0.00,0.00,0.18,0.09,0.06,0.03,0.01,0.34,0.08,0.08,0.07,0.02,0.21,0.04,0.07,0.10,0.04,0.11,0.03,0.02,0.04,0.03,0.04,0.05,0.03,0.05,0.03,0.01,0.02,0.07,0.03,0.00,0.00,0.15,0.07,0.03,0.01,0.06,0.09,0.08,0.01,0.06,0.23,0.12,0.01,0.04,0.17,0.06,0.00,0.08,0.19,0.00,0.00" 40 | 28637,"0.01,0.01,0.03,0.02,0.01,0.00,0.00,0.00,0.04,0.03,0.02,0.01,0.00,0.17,0.06,0.06,0.04,0.00,0.35,0.07,0.08,0.05,0.01,0.25,0.05,0.07,0.09,0.03,0.10,0.02,0.01,0.03,0.05,0.04,0.02,0.02,0.04,0.04,0.01,0.01,0.04,0.02,0.01,0.02,0.15,0.05,0.02,0.01,0.09,0.11,0.08,0.00,0.04,0.22,0.10,0.00,0.05,0.17,0.07,0.00,0.08,0.19,0.02,0.02" 41 | 28704,"0.02,0.02,0.03,0.02,0.01,0.00,0.02,0.00,0.04,0.04,0.03,0.00,0.00,0.18,0.06,0.05,0.00,0.00,0.38,0.06,0.08,0.03,0.01,0.27,0.03,0.08,0.08,0.03,0.10,0.02,0.03,0.04,0.03,0.03,0.01,0.02,0.04,0.03,0.00,0.01,0.06,0.03,0.01,0.00,0.12,0.04,0.02,0.00,0.08,0.06,0.07,0.00,0.03,0.15,0.11,0.00,0.04,0.13,0.06,0.00,0.09,0.17,0.01,0.00" 42 | 28771,"0.01,0.02,0.03,0.01,0.01,0.00,0.01,0.00,0.04,0.02,0.02,0.01,0.00,0.12,0.06,0.08,0.03,0.00,0.29,0.05,0.07,0.05,0.01,0.25,0.05,0.10,0.08,0.02,0.09,0.02,0.03,0.04,0.03,0.05,0.01,0.01,0.03,0.02,0.01,0.02,0.06,0.02,0.00,0.01,0.16,0.02,0.02,0.00,0.09,0.06,0.04,0.02,0.05,0.14,0.07,0.03,0.04,0.09,0.03,0.00,0.07,0.13,0.00,0.00" 43 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Processing/Soleoffline/Readme.txt: -------------------------------------------------------------------------------- 1 | Just click RUN (Play) 2 | -------------------------------------------------------------------------------- /Arduino-Processing-code/Processing/Soleoffline/Soleoffline.pde: -------------------------------------------------------------------------------- 1 | PShape sole; 2 | String myString = null; 3 | Table table1, data; 4 | int i=0,i1=0,j=0, R,G,B, ln=10, k=1, m, gain=2050, numl, a=0,b=0,c=0,d=0,e=0,f=0,g=0,h=0,x=0,x1, count=0, ff, mf, bf, bar; 5 | float val, limiter=0.25, v=0.01; 6 | float[] numsfront = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00 }; 7 | float[] numsmid = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00 }; 8 | float[] numsback = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00 }; 9 | float maxvaluefront, maxvaluemid, maxvalueback; 10 | int positionfront, positionmid, positionback; 11 | 12 | void setup() { 13 | 14 | size(1200,600); 15 | background(156); 16 | frameRate(600); 17 | 18 | table1 = loadTable("Plantar Pressure Distribution 2.csv"); 19 | } 20 | 21 | 22 | void draw() { 23 | 24 | background(156); 25 | myString = table1.getString(k,1); 26 | 27 | if (myString != null) { 28 | delay(60); 29 | float nums [] = float(split(myString, ',')); 30 | numl=nums.length; 31 | 32 | // Find Max pressure position and Values 33 | maxvaluefront = 0.00; 34 | positionfront = -1; 35 | 36 | maxvaluemid = 0.00; 37 | positionmid = -1; 38 | 39 | maxvalueback = 0.00; 40 | positionback = -1; 41 | k++; 42 | 43 | if(k>41){k=1;} 44 | 45 | if(numl>63) 46 | { 47 | 48 | for(ff=0;ff<28;ff++){ 49 | numsfront [ff]= nums[ff]; 50 | 51 | if(numsfront [ff]>maxvaluefront){ 52 | positionfront=ff; 53 | maxvaluefront=numsfront [ff]; 54 | 55 | } 56 | } 57 | 58 | for(mf=28;mf<49;mf++){ 59 | numsmid [mf-28]= nums[mf]; 60 | 61 | if(numsmid [mf-28]>maxvaluemid){ 62 | positionmid=mf; 63 | maxvaluemid=numsmid [mf-28]; 64 | } 65 | } 66 | 67 | for(bf=49;bf<63;bf++){ 68 | numsback [bf-49]= nums[bf]; 69 | 70 | 71 | if(numsback [bf-49]>maxvalueback){ 72 | positionback=bf; 73 | maxvalueback=numsback [bf-49]; 74 | } 75 | } 76 | 77 | for(bar=0;bar<64;bar++){ 78 | 79 | fill(0); 80 | stroke(0); 81 | rect (bar*10+500, 400-(nums[bar]*200), 8, (nums[bar]*200)); 82 | 83 | } 84 | 85 | textSize(16); 86 | fill(0); 87 | text("Frontfoot",610,420); 88 | 89 | textSize(16); 90 | fill(0); 91 | text("Midfoot",850,420); 92 | 93 | textSize(16); 94 | fill(0); 95 | text("Backfoot",1035,420); 96 | 97 | print("Forefoot: "); 98 | print(positionfront);print("\t");print(maxvaluefront);print("\t");print("\t"); 99 | print("Midfoot: "); 100 | print(positionmid);print("\t");print(maxvaluemid);print("\t");print("\t"); 101 | print("Backfoot: "); 102 | print(positionback);print("\t");print(maxvalueback);print("\t");println("\t"); 103 | 104 | 105 | 106 | //R1C2 107 | j=int((nums[0]-limiter)*gain); 108 | a=constrain(int(((nums[1]-limiter)*gain)*v),0,255); 109 | b=constrain(int(((nums[3]-limiter)*gain)*v),0,255); 110 | c=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 111 | d=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 112 | x=j+a+b+c+d; 113 | i=constrain(x,0,255); 114 | fill(i); 115 | stroke(i); 116 | beginShape(); 117 | //translate(100, 100, 0); 118 | //rotateY(PI/8); 119 | //rotateY(PI/4); 120 | vertex(308,3); 121 | vertex(268,38); 122 | vertex(308,38); 123 | 124 | // etc; 125 | endShape(); 126 | 127 | 128 | //R1C3 129 | j=int((nums[1]-limiter)*gain); 130 | a=constrain(int(((nums[0]-limiter)*gain)*v),0,255); 131 | b=constrain(int(((nums[2]-limiter)*gain)*v),0,255); 132 | c=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 133 | d=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 134 | e=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 135 | 136 | x=j+a+b+c+d+e; 137 | i=constrain(x,0,255); 138 | fill(i); 139 | stroke(i); 140 | beginShape(); 141 | vertex(308,3); 142 | vertex(350,5); 143 | vertex(350,39); 144 | vertex(308,39); 145 | 146 | // etc; 147 | endShape(); 148 | 149 | //R1C4 150 | j=int((nums[2]-limiter)*gain); 151 | a=constrain(int(((nums[1]-limiter)*gain)*v),0,255); 152 | b=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 153 | c=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 154 | d=constrain(int(((nums[7]-limiter)*gain)*v),0,255); 155 | x=j+a+b+c+d; 156 | i=constrain(x,0,255); 157 | fill(i); 158 | stroke(i); 159 | beginShape(); 160 | vertex(351,6); 161 | vertex(390,39); 162 | vertex(351,39); 163 | // etc; 164 | endShape(); 165 | 166 | //R2C1 167 | j=int((nums[3]-limiter)*gain); 168 | a=constrain(int(((nums[0]-limiter)*gain)*v),0,255); 169 | b=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 170 | c=constrain(int(((nums[8]-limiter)*gain)*v),0,255); 171 | d=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 172 | 173 | x=j+a+b+c+d; 174 | i=constrain(x,0,255); 175 | fill(i); 176 | stroke(i); 177 | beginShape(); 178 | vertex(266,42); 179 | vertex(249,79); 180 | vertex(266,79); 181 | // etc; 182 | endShape(); 183 | 184 | //R2C2 185 | j=int((nums[4]-limiter)*gain); 186 | a=constrain(int(((nums[0]-limiter)*gain)*v),0,255); 187 | b=constrain(int(((nums[1]-limiter)*gain)*v),0,255); 188 | c=constrain(int(((nums[3]-limiter)*gain)*v),0,255); 189 | d=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 190 | e=constrain(int(((nums[8]-limiter)*gain)*v),0,255); 191 | f=constrain(int(((nums[9]-limiter)*gain)*v),0,255);; 192 | g=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 193 | x=j+a+b+c+d+e+f+g; 194 | i=constrain(x,0,255); 195 | fill(i); 196 | stroke(i); 197 | beginShape(); 198 | vertex(266,79); 199 | vertex(308,79); 200 | vertex(308,39); 201 | vertex(266,39); 202 | // etc; 203 | endShape(); 204 | 205 | //R2C3 206 | j=constrain(int((nums[5]-limiter)*gain),0,255); 207 | a=constrain(int(((nums[0]-limiter)*gain)*v),0,255); 208 | b=constrain(int(((nums[1]-limiter)*gain)*v),0,255); 209 | c=constrain(int(((nums[2]-limiter)*gain)*v),0,255); 210 | d=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 211 | e=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 212 | f=constrain(int(((nums[9]-limiter)*gain)*v),0,255);; 213 | g=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 214 | h=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 215 | x=j+a+b+c+d+e+f+g+h; 216 | i=constrain(x,0,255); 217 | fill(i); 218 | stroke(i); 219 | beginShape(); 220 | vertex(308,79); 221 | vertex(350,79); 222 | vertex(350,39); 223 | vertex(308,39); 224 | // etc; 225 | endShape(); 226 | 227 | //R2C4 228 | j=int((nums[6]-limiter)*gain); 229 | a=constrain(int(((nums[1]-limiter)*gain)*v),0,255); 230 | b=constrain(int(((nums[2]-limiter)*gain)*v),0,255); 231 | c=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 232 | d=constrain(int(((nums[7]-limiter)*gain)*v),0,255); 233 | e=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 234 | f=constrain(int(((nums[11]-limiter)*gain)*v),0,255);; 235 | g=constrain(int(((nums[12]-limiter)*gain)*v),0,255); 236 | x=j+a+b+c+d+e+f+g; 237 | i=constrain(x,0,255); 238 | fill(i); 239 | stroke(i); 240 | beginShape(); 241 | vertex(351,78); 242 | vertex(391,78); 243 | vertex(391,39); 244 | vertex(351,39); 245 | // etc; 246 | endShape(); 247 | 248 | //R2C5 249 | j=int((nums[7]-limiter)*gain); 250 | a=constrain(int(((nums[2]-limiter)*gain)*v),0,255); 251 | b=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 252 | c=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 253 | d=constrain(int(((nums[12]-limiter)*gain)*v),0,255); 254 | x=j+a+b+c+d; 255 | i=constrain(x,0,255); 256 | fill(i); 257 | stroke(i); 258 | beginShape(); 259 | vertex(412,78); 260 | vertex(392,78); 261 | vertex(392,40); 262 | // etc; 263 | endShape(); 264 | 265 | //R3C1 266 | j=int((nums[8]-limiter)*gain); 267 | a=constrain(int(((nums[3]-limiter)*gain)*v),0,255); 268 | b=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 269 | c=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 270 | d=constrain(int(((nums[13]-limiter)*gain)*v),0,255); 271 | e=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 272 | x=j+a+b+c+d+e; 273 | i=constrain(x,0,255); 274 | fill(i); 275 | stroke(i); 276 | beginShape(); 277 | vertex(248,80); 278 | vertex(266,80); 279 | vertex(266,119); 280 | vertex(241,119); 281 | // etc; 282 | endShape(); 283 | 284 | //R3C2 285 | j=int((nums[9]-limiter)*gain); 286 | a=constrain(int(((nums[3]-limiter)*gain)*v),0,255); 287 | b=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 288 | c=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 289 | d=constrain(int(((nums[8]-limiter)*gain)*v),0,255); 290 | e=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 291 | f=constrain(int(((nums[13]-limiter)*gain)*v),0,255);; 292 | g=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 293 | h=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 294 | x=j+a+b+c+d+e+f+g+h; 295 | i=constrain(x,0,255); 296 | fill(i); 297 | stroke(i); 298 | beginShape(); 299 | vertex(308,80); 300 | vertex(266,80); 301 | vertex(266,119); 302 | vertex(308,119); 303 | // etc; 304 | endShape(); 305 | 306 | //R3C3 307 | j=int((nums[10]-limiter)*gain); 308 | a=constrain(int(((nums[4]-limiter)*gain)*v),0,255); 309 | b=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 310 | c=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 311 | d=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 312 | e=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 313 | f=constrain(int(((nums[14]-limiter)*gain)*v),0,255);; 314 | g=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 315 | h=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 316 | x=j+a+b+c+d+e+f+g+h; 317 | i=constrain(x,0,255); 318 | fill(i); 319 | stroke(i); 320 | beginShape(); 321 | vertex(351,80); 322 | vertex(308,80); 323 | vertex(308,119); 324 | vertex(351,119); 325 | // etc; 326 | endShape(); 327 | 328 | //R3C4 329 | j=int((nums[11]-limiter)*gain); 330 | a=constrain(int(((nums[5]-limiter)*gain)*v),0,255); 331 | b=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 332 | c=constrain(int(((nums[7]-limiter)*gain)*v),0,255); 333 | d=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 334 | e=constrain(int(((nums[12]-limiter)*gain)*v),0,255); 335 | f=constrain(int(((nums[15]-limiter)*gain)*v),0,255);; 336 | g=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 337 | h=constrain(int(((nums[17]-limiter)*gain)*v),0,255); 338 | x=j+a+b+c+d+e+f+g+h; 339 | i=constrain(x,0,255); 340 | fill(i); 341 | stroke(i); 342 | beginShape(); 343 | vertex(351,79); 344 | vertex(391,79); 345 | vertex(391,119); 346 | vertex(351,119); 347 | // etc; 348 | endShape(); 349 | 350 | //R3C5 351 | j=int((nums[12]-limiter)*gain); 352 | a=constrain(int(((nums[6]-limiter)*gain)*v),0,255); 353 | b=constrain(int(((nums[7]-limiter)*gain)*v),0,255); 354 | c=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 355 | d=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 356 | e=constrain(int(((nums[17]-limiter)*gain)*v),0,255); 357 | 358 | x=j+a+b+c+d+e; 359 | i=constrain(x,0,255); 360 | fill(i); 361 | stroke(i); 362 | beginShape(); 363 | vertex(413,79); 364 | vertex(392,79); 365 | vertex(392,119); 366 | vertex(427,119); 367 | // etc; 368 | endShape(); 369 | 370 | //R4C1 371 | j=int((nums[13]-limiter)*gain); 372 | a=constrain(int(((nums[8]-limiter)*gain)*v),0,255); 373 | b=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 374 | c=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 375 | d=constrain(int(((nums[18]-limiter)*gain)*v),0,255); 376 | e=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 377 | x=j+a+b+c+d+e; 378 | i=constrain(x,0,255); 379 | fill(i); 380 | stroke(i); 381 | beginShape(); 382 | vertex(241,119); 383 | vertex(266,119); 384 | vertex(266,159); 385 | vertex(240,159); 386 | // etc; 387 | endShape(); 388 | 389 | //R4C2 390 | j=int((nums[14]-limiter)*gain); 391 | a=constrain(int(((nums[8]-limiter)*gain)*v),0,255); 392 | b=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 393 | c=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 394 | d=constrain(int(((nums[13]-limiter)*gain)*v),0,255); 395 | e=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 396 | f=constrain(int(((nums[18]-limiter)*gain)*v),0,255);; 397 | g=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 398 | h=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 399 | x=j+a+b+c+d+e+f+g+h; 400 | i=constrain(x,0,255); 401 | fill(i); 402 | stroke(i); 403 | beginShape(); 404 | vertex(266,119); 405 | vertex(308,119); 406 | vertex(308,159); 407 | vertex(266,159); 408 | // etc; 409 | endShape(); 410 | 411 | //R4C3 412 | j=int((nums[15]-limiter)*gain); 413 | a=constrain(int(((nums[9]-limiter)*gain)*v),0,255); 414 | b=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 415 | c=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 416 | d=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 417 | e=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 418 | f=constrain(int(((nums[19]-limiter)*gain)*v),0,255);; 419 | g=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 420 | h=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 421 | x=j+a+b+c+d+e+f+g+h; 422 | i=constrain(x,0,255); 423 | fill(i); 424 | stroke(i); 425 | beginShape(); 426 | vertex(351,119); 427 | vertex(308,119); 428 | vertex(308,159); 429 | vertex(351,159); 430 | // etc; 431 | endShape(); 432 | 433 | //R4C4 434 | j=int((nums[16]-limiter)*gain); 435 | a=constrain(int(((nums[10]-limiter)*gain)*v),0,255); 436 | b=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 437 | c=constrain(int(((nums[12]-limiter)*gain)*v),0,255); 438 | d=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 439 | e=constrain(int(((nums[17]-limiter)*gain)*v),0,255); 440 | f=constrain(int(((nums[20]-limiter)*gain)*v),0,255);; 441 | g=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 442 | h=constrain(int(((nums[22]-limiter)*gain)*v),0,255); 443 | x=j+a+b+c+d+e+f+g+h; 444 | i=constrain(x,0,255); 445 | fill(i); 446 | stroke(i); 447 | beginShape(); 448 | vertex(351,120); 449 | vertex(391,120); 450 | vertex(391,159); 451 | vertex(351,159); 452 | // etc; 453 | endShape(); 454 | 455 | //R4C5 456 | j=int((nums[17]-limiter)*gain); 457 | a=constrain(int(((nums[11]-limiter)*gain)*v),0,255); 458 | b=constrain(int(((nums[12]-limiter)*gain)*v),0,255); 459 | c=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 460 | d=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 461 | e=constrain(int(((nums[22]-limiter)*gain)*v),0,255); 462 | 463 | x=j+a+b+c+d+e; 464 | i=constrain(x,0,255); 465 | fill(i); 466 | stroke(i); 467 | beginShape(); 468 | vertex(428,120); 469 | vertex(392,120); 470 | vertex(392,159); 471 | vertex(434,159); 472 | // etc; 473 | endShape(); 474 | 475 | //R5C1 476 | j=int((nums[18]-limiter)*gain); 477 | a=constrain(int(((nums[13]-limiter)*gain)*v),0,255); 478 | b=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 479 | c=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 480 | d=constrain(int(((nums[23]-limiter)*gain)*v),0,255); 481 | e=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 482 | 483 | x=j+a+b+c+d+e; 484 | i=constrain(x,0,255); 485 | fill(i); 486 | stroke(i); 487 | beginShape(); 488 | vertex(239,160); 489 | vertex(265,160); 490 | vertex(265,199); 491 | vertex(242,199); 492 | // etc; 493 | endShape(); 494 | 495 | //R5C2 496 | j=int((nums[19]-limiter)*gain); 497 | a=constrain(int(((nums[13]-limiter)*gain)*v),0,255); 498 | b=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 499 | c=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 500 | d=constrain(int(((nums[18]-limiter)*gain)*v),0,255); 501 | e=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 502 | f=constrain(int(((nums[23]-limiter)*gain)*v),0,255);; 503 | g=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 504 | h=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 505 | x=j+a+b+c+d+e+f+g+h; 506 | i=constrain(x,0,255); 507 | fill(i); 508 | stroke(i); 509 | beginShape(); 510 | vertex(309,160); 511 | vertex(266,160); 512 | vertex(266,199); 513 | vertex(309,199); 514 | // etc; 515 | endShape(); 516 | 517 | //R5C3 518 | j=int((nums[20]-limiter)*gain); 519 | a=constrain(int(((nums[14]-limiter)*gain)*v),0,255); 520 | b=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 521 | c=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 522 | d=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 523 | e=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 524 | f=constrain(int(((nums[24]-limiter)*gain)*v),0,255);; 525 | g=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 526 | h=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 527 | x=j+a+b+c+d+e+f+g+h; 528 | i=constrain(x,0,255); 529 | fill(i); 530 | stroke(i); 531 | beginShape(); 532 | vertex(351,160); 533 | vertex(309,160); 534 | vertex(309,199); 535 | vertex(351,199); 536 | // etc; 537 | endShape(); 538 | 539 | //R5C4 540 | j=int((nums[21]-limiter)*gain); 541 | a=constrain(int(((nums[15]-limiter)*gain)*v),0,255); 542 | b=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 543 | c=constrain(int(((nums[17]-limiter)*gain)*v),0,255); 544 | d=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 545 | e=constrain(int(((nums[22]-limiter)*gain)*v),0,255); 546 | f=constrain(int(((nums[25]-limiter)*gain)*v),0,255);; 547 | g=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 548 | h=constrain(int(((nums[27]-limiter)*gain)*v),0,255); 549 | x=j+a+b+c+d+e+f+g+h; 550 | i=constrain(x,0,255); 551 | fill(i); 552 | stroke(i); 553 | beginShape(); 554 | vertex(351,160); 555 | vertex(391,160); 556 | vertex(391,199); 557 | vertex(351,199); 558 | // etc; 559 | endShape(); 560 | 561 | //R5C5 562 | j=int((nums[22]-limiter)*gain); 563 | a=constrain(int(((nums[16]-limiter)*gain)*v),0,255); 564 | b=constrain(int(((nums[17]-limiter)*gain)*v),0,255); 565 | c=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 566 | d=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 567 | e=constrain(int(((nums[27]-limiter)*gain)*v),0,255); 568 | 569 | x=j+a+b+c+d+e; 570 | i=constrain(x,0,255); 571 | fill(i); 572 | stroke(i); 573 | beginShape(); 574 | vertex(435,160); 575 | vertex(392,160); 576 | vertex(392,199); 577 | vertex(438,199); 578 | // etc; 579 | endShape(); 580 | 581 | //R6C1 582 | j=int((nums[23]-limiter)*gain); 583 | a=constrain(int(((nums[18]-limiter)*gain)*v),0,255); 584 | b=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 585 | c=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 586 | d=constrain(int(((nums[28]-limiter)*gain)*v),0,255); 587 | e=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 588 | 589 | x=j+a+b+c+d+e; 590 | i=constrain(x,0,255); 591 | fill(i); 592 | stroke(i); 593 | beginShape(); 594 | vertex(266,200); 595 | vertex(243,200); 596 | vertex(252,239); 597 | vertex(266,239); 598 | // etc; 599 | endShape(); 600 | 601 | //R6C2 602 | j=int((nums[24]-limiter)*gain); 603 | a=constrain(int(((nums[18]-limiter)*gain)*v),0,255); 604 | b=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 605 | c=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 606 | d=constrain(int(((nums[23]-limiter)*gain)*v),0,255); 607 | e=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 608 | f=constrain(int(((nums[28]-limiter)*gain)*v),0,255);; 609 | g=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 610 | h=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 611 | x=j+a+b+c+d+e+f+g+h; 612 | i=constrain(x,0,255); 613 | fill(i); 614 | stroke(i); 615 | beginShape(); 616 | vertex(308,200); 617 | vertex(266,200); 618 | vertex(266,239); 619 | vertex(308,239); 620 | // etc; 621 | endShape(); 622 | 623 | //R6C3 624 | j=int((nums[25]-limiter)*gain); 625 | a=constrain(int(((nums[19]-limiter)*gain)*v),0,255); 626 | b=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 627 | c=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 628 | d=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 629 | e=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 630 | f=constrain(int(((nums[29]-limiter)*gain)*v),0,255);; 631 | g=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 632 | h=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 633 | x=j+a+b+c+d+e+f+g+h; 634 | i=constrain(x,0,255); 635 | fill(i); 636 | stroke(i); 637 | beginShape(); 638 | vertex(351,200); 639 | vertex(308,200); 640 | vertex(308,239); 641 | vertex(351,239); 642 | // etc; 643 | endShape(); 644 | 645 | //R6C4 646 | j=int((nums[26]-limiter)*gain); 647 | a=constrain(int(((nums[20]-limiter)*gain)*v),0,255); 648 | b=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 649 | c=constrain(int(((nums[22]-limiter)*gain)*v),0,255); 650 | d=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 651 | e=constrain(int(((nums[27]-limiter)*gain)*v),0,255); 652 | f=constrain(int(((nums[30]-limiter)*gain)*v),0,255);; 653 | g=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 654 | h=constrain(int(((nums[32]-limiter)*gain)*v),0,255); 655 | x=j+a+b+c+d+e+f+g+h; 656 | i=constrain(x,0,255); 657 | fill(i); 658 | stroke(i); 659 | beginShape(); 660 | vertex(351,200); 661 | vertex(391,200); 662 | vertex(391,239); 663 | vertex(351,239); 664 | // etc; 665 | endShape(); 666 | 667 | //R6C5 668 | j=int((nums[27]-limiter)*gain); 669 | a=constrain(int(((nums[21]-limiter)*gain)*v),0,255); 670 | b=constrain(int(((nums[22]-limiter)*gain)*v),0,255); 671 | c=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 672 | d=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 673 | e=constrain(int(((nums[32]-limiter)*gain)*v),0,255); 674 | x=j+a+b+c+d+e; 675 | i=constrain(x,0,255); 676 | fill(i); 677 | stroke(i); 678 | beginShape(); 679 | vertex(438,200); 680 | vertex(392,200); 681 | vertex(392,239); 682 | vertex(437,239); 683 | // etc; 684 | endShape(); 685 | 686 | //R7C1 687 | j=int((nums[28]-limiter)*gain); 688 | a=constrain(int(((nums[23]-limiter)*gain)*v),0,255); 689 | b=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 690 | c=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 691 | d=constrain(int(((nums[33]-limiter)*gain)*v),0,255); 692 | 693 | x=j+a+b+c+d; 694 | i=constrain(x,0,255); 695 | fill(i); 696 | stroke(i); 697 | beginShape(); 698 | vertex(266,240); 699 | vertex(252,240); 700 | vertex(266,278); 701 | // etc; 702 | endShape(); 703 | 704 | //R7C2 705 | j=int((nums[29]-limiter)*gain); 706 | a=constrain(int(((nums[23]-limiter)*gain)*v),0,255); 707 | b=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 708 | c=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 709 | d=constrain(int(((nums[28]-limiter)*gain)*v),0,255); 710 | e=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 711 | f=constrain(int(((nums[33]-limiter)*gain)*v),0,255);; 712 | g=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 713 | x=j+a+b+c+d+e+f+g; 714 | i=constrain(x,0,255); 715 | fill(i); 716 | stroke(i); 717 | beginShape(); 718 | vertex(308,240); 719 | vertex(266,240); 720 | vertex(266,279); 721 | vertex(308,279); 722 | // etc; 723 | endShape(); 724 | 725 | //R7C3 726 | j=int((nums[30]-limiter)*gain); 727 | a=constrain(int(((nums[24]-limiter)*gain)*v),0,255); 728 | b=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 729 | c=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 730 | d=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 731 | e=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 732 | f=constrain(int(((nums[33]-limiter)*gain)*v),0,255);; 733 | g=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 734 | h=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 735 | x=j+a+b+c+d+e+f+g+h; 736 | i=constrain(x,0,255); 737 | fill(i); 738 | stroke(i); 739 | beginShape(); 740 | vertex(351,240); 741 | vertex(308,240); 742 | vertex(308,279); 743 | vertex(351,279); 744 | // etc; 745 | endShape(); 746 | 747 | //R7C4 748 | j=int((nums[31]-limiter)*gain); 749 | a=constrain(int(((nums[25]-limiter)*gain)*v),0,255); 750 | b=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 751 | c=constrain(int(((nums[27]-limiter)*gain)*v),0,255); 752 | d=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 753 | e=constrain(int(((nums[32]-limiter)*gain)*v),0,255); 754 | f=constrain(int(((nums[34]-limiter)*gain)*v),0,255);; 755 | g=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 756 | h=constrain(int(((nums[36]-limiter)*gain)*v),0,255); 757 | x=j+a+b+c+d+e+f+g+h; 758 | i=constrain(x,0,255); 759 | fill(i); 760 | stroke(i); 761 | beginShape(); 762 | vertex(351,240); 763 | vertex(391,240); 764 | vertex(391,279); 765 | vertex(351,279); 766 | // etc; 767 | endShape(); 768 | 769 | //R7C5 770 | j=int((nums[32]-limiter)*gain); 771 | a=constrain(int(((nums[26]-limiter)*gain)*v),0,255); 772 | b=constrain(int(((nums[27]-limiter)*gain)*v),0,255); 773 | c=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 774 | d=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 775 | e=constrain(int(((nums[36]-limiter)*gain)*v),0,255); 776 | 777 | x=j+a+b+c+d+e; 778 | i=constrain(x,0,255); 779 | fill(i); 780 | stroke(i); 781 | beginShape(); 782 | vertex(437,240); 783 | vertex(392,240); 784 | vertex(392,279); 785 | vertex(433,279); 786 | // etc; 787 | endShape(); 788 | 789 | //R8C2 790 | j=int((nums[33]-limiter)*gain); 791 | a=constrain(int(((nums[28]-limiter)*gain)*v),0,255); 792 | b=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 793 | c=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 794 | d=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 795 | e=constrain(int(((nums[37]-limiter)*gain)*v),0,255); 796 | f=constrain(int(((nums[38]-limiter)*gain)*v),0,255);; 797 | 798 | x=j+a+b+c+d+e+f; 799 | i=constrain(x,0,255); 800 | fill(i); 801 | stroke(i); 802 | beginShape(); 803 | vertex(308,280); 804 | vertex(267,280); 805 | vertex(284,321); 806 | vertex(308,321); 807 | // etc; 808 | endShape(); 809 | 810 | //R8C3 811 | j=int((nums[34]-limiter)*gain); 812 | a=constrain(int(((nums[29]-limiter)*gain)*v),0,255); 813 | b=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 814 | c=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 815 | d=constrain(int(((nums[33]-limiter)*gain)*v),0,255); 816 | e=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 817 | f=constrain(int(((nums[37]-limiter)*gain)*v),0,255);; 818 | g=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 819 | h=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 820 | x=j+a+b+c+d+e+f+g+h; 821 | i=constrain(x,0,255); 822 | fill(i); 823 | stroke(i); 824 | beginShape(); 825 | vertex(351,280); 826 | vertex(308,280); 827 | vertex(308,321); 828 | vertex(351,321); 829 | // etc; 830 | endShape(); 831 | 832 | //R8C4 833 | j=int((nums[35]-limiter)*gain); 834 | a=constrain(int(((nums[30]-limiter)*gain)*v),0,255); 835 | b=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 836 | c=constrain(int(((nums[32]-limiter)*gain)*v),0,255); 837 | d=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 838 | e=constrain(int(((nums[36]-limiter)*gain)*v),0,255); 839 | f=constrain(int(((nums[38]-limiter)*gain)*v),0,255);; 840 | g=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 841 | h=constrain(int(((nums[40]-limiter)*gain)*v),0,255); 842 | x=j+a+b+c+d+e+f+g+h; 843 | i=constrain(x,0,255); 844 | fill(i); 845 | stroke(i); 846 | beginShape(); 847 | vertex(351,280); 848 | vertex(391,280); 849 | vertex(391,321); 850 | vertex(351,321); 851 | // etc; 852 | endShape(); 853 | 854 | //R8C5 855 | j=int((nums[36]-limiter)*gain); 856 | a=constrain(int(((nums[31]-limiter)*gain)*v),0,255); 857 | b=constrain(int(((nums[32]-limiter)*gain)*v),0,255); 858 | c=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 859 | d=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 860 | e=constrain(int(((nums[40]-limiter)*gain)*v),0,255); 861 | 862 | x=j+a+b+c+d+e; 863 | i=constrain(x,0,255); 864 | fill(i); 865 | stroke(i); 866 | beginShape(); 867 | vertex(433,280); 868 | vertex(392,280); 869 | vertex(392,321); 870 | vertex(426,321); 871 | // etc; 872 | endShape(); 873 | 874 | //R9C2 875 | j=int((nums[37]-limiter)*gain); 876 | a=constrain(int(((nums[33]-limiter)*gain)*v),0,255); 877 | b=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 878 | c=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 879 | d=constrain(int(((nums[41]-limiter)*gain)*v),0,255); 880 | e=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 881 | 882 | x=j+a+b+c+d+e; 883 | i=constrain(x,0,255); 884 | fill(i); 885 | stroke(i); 886 | beginShape(); 887 | vertex(308,322); 888 | vertex(284,322); 889 | vertex(287,361); 890 | vertex(308,361); 891 | // etc; 892 | endShape(); 893 | 894 | //R9C3 895 | j=int((nums[38]-limiter)*gain); 896 | a=constrain(int(((nums[33]-limiter)*gain)*v),0,255); 897 | b=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 898 | c=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 899 | d=constrain(int(((nums[37]-limiter)*gain)*v),0,255); 900 | e=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 901 | f=constrain(int(((nums[41]-limiter)*gain)*v),0,255);; 902 | g=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 903 | h=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 904 | x=j+a+b+c+d+e+f+g+h; 905 | i=constrain(x,0,255); 906 | fill(i); 907 | stroke(i); 908 | beginShape(); 909 | vertex(351,322); 910 | vertex(308,322); 911 | vertex(308,361); 912 | vertex(351,361); 913 | // etc; 914 | endShape(); 915 | 916 | //R9C4 917 | j=int((nums[39]-limiter)*gain); 918 | a=constrain(int(((nums[34]-limiter)*gain)*v),0,255); 919 | b=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 920 | c=constrain(int(((nums[36]-limiter)*gain)*v),0,255); 921 | d=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 922 | e=constrain(int(((nums[40]-limiter)*gain)*v),0,255); 923 | f=constrain(int(((nums[42]-limiter)*gain)*v),0,255);; 924 | g=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 925 | h=constrain(int(((nums[44]-limiter)*gain)*v),0,255); 926 | x=j+a+b+c+d+e+f+g+h; 927 | i=constrain(x,0,255); 928 | fill(i); 929 | stroke(i); 930 | beginShape(); 931 | vertex(351,322); 932 | vertex(391,322); 933 | vertex(391,361); 934 | vertex(351,361); 935 | // etc; 936 | endShape(); 937 | 938 | //R9C5 939 | j=int((nums[40]-limiter)*gain); 940 | a=constrain(int(((nums[35]-limiter)*gain)*v),0,255); 941 | b=constrain(int(((nums[36]-limiter)*gain)*v),0,255); 942 | c=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 943 | d=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 944 | e=constrain(int(((nums[44]-limiter)*gain)*v),0,255); 945 | 946 | x=j+a+b+c+d+e; 947 | i=constrain(x,0,255); 948 | fill(i); 949 | stroke(i); 950 | beginShape(); 951 | vertex(426,322); 952 | vertex(392,322); 953 | vertex(392,361); 954 | vertex(420,361); 955 | // etc; 956 | endShape(); 957 | 958 | //R10C2 959 | j=int((nums[41]-limiter)*gain); 960 | a=constrain(int(((nums[37]-limiter)*gain)*v),0,255); 961 | b=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 962 | c=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 963 | d=constrain(int(((nums[45]-limiter)*gain)*v),0,255); 964 | e=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 965 | 966 | x=j+a+b+c+d+e; 967 | i=constrain(x,0,255); 968 | fill(i); 969 | stroke(i); 970 | beginShape(); 971 | vertex(308,362); 972 | vertex(288,362); 973 | vertex(282,400); 974 | vertex(308,400); 975 | // etc; 976 | endShape(); 977 | 978 | //R10C3 979 | j=int((nums[42]-limiter)*gain); 980 | a=constrain(int(((nums[37]-limiter)*gain)*v),0,255); 981 | b=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 982 | c=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 983 | d=constrain(int(((nums[41]-limiter)*gain)*v),0,255); 984 | e=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 985 | f=constrain(int(((nums[45]-limiter)*gain)*v),0,255);; 986 | g=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 987 | h=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 988 | x=j+a+b+c+d+e+f+g+h; 989 | i=constrain(x,0,255); 990 | fill(i); 991 | stroke(i); 992 | beginShape(); 993 | vertex(351,362); 994 | vertex(308,362); 995 | vertex(308,400); 996 | vertex(351,400); 997 | // etc; 998 | endShape(); 999 | 1000 | //R10C4 1001 | j=int((nums[43]-limiter)*gain); 1002 | a=constrain(int(((nums[38]-limiter)*gain)*v),0,255); 1003 | b=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 1004 | c=constrain(int(((nums[40]-limiter)*gain)*v),0,255); 1005 | d=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 1006 | e=constrain(int(((nums[44]-limiter)*gain)*v),0,255); 1007 | f=constrain(int(((nums[46]-limiter)*gain)*v),0,255);; 1008 | g=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1009 | h=constrain(int(((nums[48]-limiter)*gain)*v),0,255); 1010 | x=j+a+b+c+d+e+f+g+h; 1011 | i=constrain(x,0,255); 1012 | fill(i); 1013 | stroke(i); 1014 | beginShape(); 1015 | vertex(351,362); 1016 | vertex(391,362); 1017 | vertex(391,400); 1018 | vertex(351,400); 1019 | // etc; 1020 | endShape(); 1021 | 1022 | //R10C5 1023 | j=int((nums[44]-limiter)*gain); 1024 | a=constrain(int(((nums[39]-limiter)*gain)*v),0,255); 1025 | b=constrain(int(((nums[40]-limiter)*gain)*v),0,255); 1026 | c=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 1027 | d=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1028 | e=constrain(int(((nums[48]-limiter)*gain)*v),0,255); 1029 | 1030 | x=j+a+b+c+d+e; 1031 | i=constrain(x,0,255); 1032 | fill(i); 1033 | stroke(i); 1034 | beginShape(); 1035 | vertex(420,362); 1036 | vertex(392,362); 1037 | vertex(392,400); 1038 | vertex(417,400); 1039 | // etc; 1040 | endShape(); 1041 | 1042 | //R11C2 1043 | j=int((nums[45]-limiter)*gain); 1044 | a=constrain(int(((nums[41]-limiter)*gain)*v),0,255); 1045 | b=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 1046 | c=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 1047 | d=constrain(int(((nums[49]-limiter)*gain)*v),0,255); 1048 | e=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1049 | 1050 | x=j+a+b+c+d+e; 1051 | i=constrain(x,0,255); 1052 | fill(i); 1053 | stroke(i); 1054 | beginShape(); 1055 | vertex(308,401); 1056 | vertex(282,401); 1057 | vertex(273,441); 1058 | vertex(308,441); 1059 | // etc; 1060 | endShape(); 1061 | 1062 | //R11C3 1063 | j=int((nums[46]-limiter)*gain); 1064 | a=constrain(int(((nums[41]-limiter)*gain)*v),0,255); 1065 | b=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 1066 | c=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 1067 | d=constrain(int(((nums[45]-limiter)*gain)*v),0,255); 1068 | e=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1069 | f=constrain(int(((nums[49]-limiter)*gain)*v),0,255);; 1070 | g=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1071 | h=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1072 | x=j+a+b+c+d+e+f+g+h; 1073 | i=constrain(x,0,255); 1074 | fill(i); 1075 | stroke(i); 1076 | beginShape(); 1077 | vertex(351,401); 1078 | vertex(308,401); 1079 | vertex(308,441); 1080 | vertex(351,441); 1081 | // etc; 1082 | endShape(); 1083 | 1084 | //R11C4 1085 | j=int((nums[47]-limiter)*gain); 1086 | a=constrain(int(((nums[42]-limiter)*gain)*v),0,255); 1087 | b=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 1088 | c=constrain(int(((nums[44]-limiter)*gain)*v),0,255); 1089 | d=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 1090 | e=constrain(int(((nums[48]-limiter)*gain)*v),0,255); 1091 | f=constrain(int(((nums[50]-limiter)*gain)*v),0,255);; 1092 | g=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1093 | h=constrain(int(((nums[52]-limiter)*gain)*v),0,255); 1094 | x=j+a+b+c+d+e+f+g+h; 1095 | i=constrain(x,0,255); 1096 | fill(i); 1097 | stroke(i); 1098 | beginShape(); 1099 | vertex(351,401); 1100 | vertex(391,401); 1101 | vertex(391,441); 1102 | vertex(351,441); 1103 | // etc; 1104 | endShape(); 1105 | 1106 | //R11C5 1107 | j=int((nums[48]-limiter)*gain); 1108 | a=constrain(int(((nums[43]-limiter)*gain)*v),0,255); 1109 | b=constrain(int(((nums[44]-limiter)*gain)*v),0,255); 1110 | c=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1111 | d=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1112 | e=constrain(int(((nums[52]-limiter)*gain)*v),0,255); 1113 | 1114 | x=j+a+b+c+d+e; 1115 | i=constrain(x,0,255); 1116 | fill(i); 1117 | stroke(i); 1118 | beginShape(); 1119 | vertex(417,401); 1120 | vertex(392,401); 1121 | vertex(392,441); 1122 | vertex(415,441); 1123 | // etc; 1124 | endShape(); 1125 | 1126 | //R12C2 1127 | j=int((nums[49]-limiter)*gain); 1128 | a=constrain(int(((nums[45]-limiter)*gain)*v),0,255); 1129 | b=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 1130 | c=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1131 | d=constrain(int(((nums[53]-limiter)*gain)*v),0,255); 1132 | e=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1133 | 1134 | x=j+a+b+c+d+e; 1135 | i=constrain(x,0,255); 1136 | fill(i); 1137 | stroke(i); 1138 | beginShape(); 1139 | vertex(308,442); 1140 | vertex(273,442); 1141 | vertex(267,482); 1142 | vertex(308,482); 1143 | // etc; 1144 | endShape(); 1145 | 1146 | //R12C3 1147 | j=int((nums[50]-limiter)*gain); 1148 | a=constrain(int(((nums[45]-limiter)*gain)*v),0,255); 1149 | b=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 1150 | c=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1151 | d=constrain(int(((nums[49]-limiter)*gain)*v),0,255); 1152 | e=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1153 | f=constrain(int(((nums[53]-limiter)*gain)*v),0,255);; 1154 | g=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1155 | h=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1156 | x=j+a+b+c+d+e+f+g+h; 1157 | i=constrain(x,0,255); 1158 | fill(i); 1159 | stroke(i); 1160 | beginShape(); 1161 | vertex(351,442); 1162 | vertex(308,442); 1163 | vertex(308,482); 1164 | vertex(351,482); 1165 | // etc; 1166 | endShape(); 1167 | 1168 | //R12C4 1169 | j=int((nums[51]-limiter)*gain); 1170 | a=constrain(int(((nums[46]-limiter)*gain)*v),0,255); 1171 | b=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1172 | c=constrain(int(((nums[48]-limiter)*gain)*v),0,255); 1173 | d=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1174 | e=constrain(int(((nums[52]-limiter)*gain)*v),0,255); 1175 | f=constrain(int(((nums[54]-limiter)*gain)*v),0,255);; 1176 | g=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1177 | h=constrain(int(((nums[56]-limiter)*gain)*v),0,255); 1178 | x=j+a+b+c+d+e+f+g+h; 1179 | i=constrain(x,0,255); 1180 | fill(i); 1181 | stroke(i); 1182 | beginShape(); 1183 | vertex(351,442); 1184 | vertex(391,442); 1185 | vertex(391,482); 1186 | vertex(351,482); 1187 | // etc; 1188 | endShape(); 1189 | 1190 | //R12C5 1191 | j=int((nums[52]-limiter)*gain); 1192 | a=constrain(int(((nums[47]-limiter)*gain)*v),0,255); 1193 | b=constrain(int(((nums[48]-limiter)*gain)*v),0,255); 1194 | c=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1195 | d=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1196 | e=constrain(int(((nums[56]-limiter)*gain)*v),0,255); 1197 | 1198 | x=j+a+b+c+d+e; 1199 | i=constrain(x,0,255); 1200 | fill(i); 1201 | stroke(i); 1202 | beginShape(); 1203 | vertex(415,442); 1204 | vertex(392,442); 1205 | vertex(392,482); 1206 | vertex(415,482); 1207 | // etc; 1208 | endShape(); 1209 | 1210 | //R13C2 1211 | j=int((nums[53]-limiter)*gain); 1212 | a=constrain(int(((nums[49]-limiter)*gain)*v),0,255); 1213 | b=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1214 | c=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1215 | d=constrain(int(((nums[57]-limiter)*gain)*v),0,255); 1216 | e=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1217 | 1218 | x=j+a+b+c+d+e; 1219 | i=constrain(x,0,255); 1220 | fill(i); 1221 | stroke(i); 1222 | beginShape(); 1223 | vertex(308,483); 1224 | vertex(266,483); 1225 | vertex(266,521); 1226 | vertex(308,521); 1227 | // etc; 1228 | endShape(); 1229 | 1230 | //R13C3 1231 | j=int((nums[54]-limiter)*gain); 1232 | a=constrain(int(((nums[49]-limiter)*gain)*v),0,255); 1233 | b=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1234 | c=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1235 | d=constrain(int(((nums[53]-limiter)*gain)*v),0,255); 1236 | e=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1237 | f=constrain(int(((nums[57]-limiter)*gain)*v),0,255);; 1238 | g=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1239 | h=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1240 | x=j+a+b+c+d+e+f+g+h; 1241 | i=constrain(x,0,255); 1242 | fill(i); 1243 | stroke(i); 1244 | beginShape(); 1245 | vertex(351,483); 1246 | vertex(308,483); 1247 | vertex(308,521); 1248 | vertex(351,521); 1249 | // etc; 1250 | endShape(); 1251 | 1252 | //R13C4 1253 | j=int((nums[55]-limiter)*gain); 1254 | a=constrain(int(((nums[50]-limiter)*gain)*v),0,255); 1255 | b=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1256 | c=constrain(int(((nums[52]-limiter)*gain)*v),0,255); 1257 | d=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1258 | e=constrain(int(((nums[56]-limiter)*gain)*v),0,255); 1259 | f=constrain(int(((nums[58]-limiter)*gain)*v),0,255);; 1260 | g=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1261 | h=constrain(int(((nums[60]-limiter)*gain)*v),0,255); 1262 | x=j+a+b+c+d+e+f+g+h; 1263 | i=constrain(x,0,255); 1264 | fill(i); 1265 | stroke(i); 1266 | beginShape(); 1267 | vertex(351,483); 1268 | vertex(391,483); 1269 | vertex(391,521); 1270 | vertex(351,521); 1271 | // etc; 1272 | endShape(); 1273 | 1274 | //R13C5 1275 | j=int((nums[56]-limiter)*gain); 1276 | a=constrain(int(((nums[51]-limiter)*gain)*v),0,255); 1277 | b=constrain(int(((nums[52]-limiter)*gain)*v),0,255); 1278 | c=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1279 | d=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1280 | e=constrain(int(((nums[60]-limiter)*gain)*v),0,255); 1281 | 1282 | x=j+a+b+c+d+e; 1283 | i=constrain(x,0,255); 1284 | fill(i); 1285 | stroke(i); 1286 | beginShape(); 1287 | vertex(415,483); 1288 | vertex(392,483); 1289 | vertex(392,521); 1290 | vertex(412,521); 1291 | // etc; 1292 | endShape(); 1293 | 1294 | //R14C2 1295 | j=int((nums[57]-limiter)*gain); 1296 | a=constrain(int(((nums[53]-limiter)*gain)*v),0,255); 1297 | b=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1298 | c=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1299 | d=constrain(int(((nums[61]-limiter)*gain)*v),0,255); 1300 | e=constrain(int(((nums[62]-limiter)*gain)*v),0,255); 1301 | 1302 | x=j+a+b+c+d+e; 1303 | i=constrain(x,0,255); 1304 | fill(i); 1305 | stroke(i); 1306 | beginShape(); 1307 | vertex(308,522); 1308 | vertex(266,522); 1309 | vertex(275,560); 1310 | vertex(308,560); 1311 | // etc; 1312 | endShape(); 1313 | 1314 | //R14C3 1315 | j=int((nums[58]-limiter)*gain); 1316 | a=constrain(int(((nums[53]-limiter)*gain)*v),0,255); 1317 | b=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1318 | c=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1319 | d=constrain(int(((nums[57]-limiter)*gain)*v),0,255); 1320 | e=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1321 | f=constrain(int(((nums[61]-limiter)*gain)*v),0,255);; 1322 | g=constrain(int(((nums[62]-limiter)*gain)*v),0,255); 1323 | h=constrain(int(((nums[63]-limiter)*gain)*v),0,255); 1324 | x=j+a+b+c+d+e+f+g+h; 1325 | i=constrain(x,0,255); 1326 | fill(i); 1327 | stroke(i); 1328 | beginShape(); 1329 | vertex(351,522); 1330 | vertex(308,522); 1331 | vertex(308,560); 1332 | vertex(351,560); 1333 | // etc; 1334 | endShape(); 1335 | 1336 | //R14C4 1337 | j=int((nums[59]-limiter)*gain); 1338 | a=constrain(int(((nums[54]-limiter)*gain)*v),0,255); 1339 | b=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1340 | c=constrain(int(((nums[56]-limiter)*gain)*v),0,255); 1341 | d=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1342 | e=constrain(int(((nums[60]-limiter)*gain)*v),0,255); 1343 | f=constrain(int(((nums[62]-limiter)*gain)*v),0,255);; 1344 | g=constrain(int(((nums[63]-limiter)*gain)*v),0,255); 1345 | x=j+a+b+c+d+e+f+g; 1346 | i=constrain(x,0,255); 1347 | fill(i); 1348 | stroke(i); 1349 | beginShape(); 1350 | vertex(351,522); 1351 | vertex(391,522); 1352 | vertex(391,560); 1353 | vertex(351,560); 1354 | // etc; 1355 | endShape(); 1356 | 1357 | //R14C5 1358 | j=int((nums[60]-limiter)*gain); 1359 | a=constrain(int(((nums[55]-limiter)*gain)*v),0,255); 1360 | b=constrain(int(((nums[56]-limiter)*gain)*v),0,255); 1361 | c=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1362 | d=constrain(int(((nums[63]-limiter)*gain)*v),0,255); 1363 | x=j+a+b+c+d; 1364 | i=constrain(x,0,255); 1365 | fill(i); 1366 | stroke(i); 1367 | beginShape(); 1368 | vertex(411,522); 1369 | vertex(392,522); 1370 | vertex(392,560); 1371 | // etc; 1372 | endShape(); 1373 | 1374 | //R15C2 1375 | j=int((nums[61]-limiter)*gain); 1376 | a=constrain(int(((nums[57]-limiter)*gain)*v),0,255); 1377 | b=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1378 | c=constrain(int(((nums[62]-limiter)*gain)*v),0,255); 1379 | 1380 | x=j+a+b+c; 1381 | i=constrain(x,0,255); 1382 | fill(i); 1383 | stroke(i); 1384 | beginShape(); 1385 | vertex(308,561); 1386 | vertex(277,561); 1387 | vertex(308,593); 1388 | // etc; 1389 | endShape(); 1390 | 1391 | //R15C3 1392 | j=int((nums[62]-limiter)*gain); 1393 | a=constrain(int(((nums[57]-limiter)*gain)*v),0,255); 1394 | b=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1395 | c=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1396 | d=constrain(int(((nums[61]-limiter)*gain)*v),0,255); 1397 | e=constrain(int(((nums[63]-limiter)*gain)*v),0,255); 1398 | 1399 | x=j+a+b+c+d+e; 1400 | i=constrain(x,0,255); 1401 | fill(i); 1402 | stroke(i); 1403 | beginShape(); 1404 | vertex(351,561); 1405 | vertex(308,561); 1406 | vertex(308,593); 1407 | vertex(351,595); 1408 | // etc; 1409 | endShape(); 1410 | 1411 | 1412 | //R15C4 1413 | j=int((nums[63]-limiter)*gain); 1414 | a=constrain(int(((nums[58]-limiter)*gain)*v),0,255); 1415 | b=constrain(int(((nums[59]-limiter)*gain)*v),0,255); 1416 | c=constrain(int(((nums[60]-limiter)*gain)*v),0,255); 1417 | d=constrain(int(((nums[62]-limiter)*gain)*v),0,255); 1418 | 1419 | x=j+a+b+c+d; 1420 | i=constrain(x,0,255); 1421 | fill(i); 1422 | stroke(i); 1423 | beginShape(); 1424 | vertex(351,561); 1425 | vertex(390,561); 1426 | vertex(351,595); 1427 | // etc; 1428 | endShape(); 1429 | 1430 | } 1431 | } 1432 | } 1433 | -------------------------------------------------------------------------------- /Demo_Video.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Demo_Video.mp4 -------------------------------------------------------------------------------- /Gait.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Gait.png -------------------------------------------------------------------------------- /Gait_Phases.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Gait_Phases.png -------------------------------------------------------------------------------- /Gait_cycle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Gait_cycle.jpg -------------------------------------------------------------------------------- /IMU.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/IMU.jpg -------------------------------------------------------------------------------- /Insole Visualization.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Insole Visualization.gif -------------------------------------------------------------------------------- /Insole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Insole.png -------------------------------------------------------------------------------- /Insole_Regions_one_Gait_cycle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Insole_Regions_one_Gait_cycle.png -------------------------------------------------------------------------------- /Kinovea.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Kinovea.mp4 -------------------------------------------------------------------------------- /Kinovea.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/Kinovea.png -------------------------------------------------------------------------------- /PP.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArvindChandran/Sensor-Development-for-Gait-Analysis-/9fd5cae9ad8964aaaecda3af85a7a8cbc0b26227/PP.mp4 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Welcome to our Project 2 | 3 | ## Wearable-Sensor-Systems-Development-for-Gait-Analysis 4 | Gait analysis is a systematic evaluation of human motion, and serves as an important medical diagnostic process which has many end user applications in healthcare, rehabilitation therapy and exercise training. The technology supporting the analysis of human motion has advanced over time. However conventional gait analysis must be performed in a gait laboratory, which is costly and hence inaccessible to most people, and also cannot provide gait measurement in a natural terrain. In recent years, demand for wearable sensor systems for the purpose of gait analysis has taken a huge leap as a cost-effective technology and has also shown promising results. 5 | 6 |

7 | 8 |

9 | 10 | This project is aimed at presenting a wearable sensor platform for gait data acquisition and analysis. The first sensor system includes an in-shoe plantar pressure sensor insole and inertial measurement units. The in-shoe system includes a sensorized insole with a piezo resistive material. Key features of this insole include good working pressure range, cost effectiveness, real time data visualization (check Insole Visualization.gif or PP.mp4) and data acquisition for an intuitive understanding of plantar pressure distribution. 11 | 12 | ### Plantar Pressure Insole 13 | ![Sensor-Development-for-Gait-Analysis-](./Insole.png) 14 | 15 | The regions in the feet are subdivided into eight anatomical regions and the mean pressure is plotted over time which is crucial for gait event detection (Gait Phases). 16 | 17 | ### Insole Data over One Gait Cycle 18 | ![Sensor-Development-for-Gait-Analysis-](./Insole_Regions_one_Gait_cycle.png) 19 | 20 | The second sensor system involves inertial measuring units for body segment and joint angle estimation in the context of human motion analysis. The main focus is on angle estimation methods that use only accelerometers and gyroscopes and therefore do not rely on homogenous magnetic fields. The anglular changes are estimated by fusing GyroRate(°/sec) and Accelerometer angle (calculated) using Kalman and Complementary filter algorithms. 21 | 22 | IMU Sensor System (MPU-9250) 23 | ![Sensor-Development-for-Gait-Analysis-](./IMU.jpg) 24 | 25 | The estimated angles of the gait trails are validated with an optical reference system using Kinovea. Data acquisition includes a set of experiments of real life scenarios like sitting and standing postures, hallway walking from which gait parameters and features are extracted. Finally, the work is concluded with estimation of gait parameters which include gait phase events, body segment orientation, joint angle and temporal parameters like stride time, step time, cadence, stance time, swing time etc. 26 | 27 | ### Gait Cycle Standards (Rancho Los Amigos (RLA) gait analysis committee) 28 | ![Sensor-Development-for-Gait-Analysis-](./Gait_cycle.jpg) 29 | 30 | 31 | 32 | ### Gait Event Detection 33 | ![Sensor-Development-for-Gait-Analysis-](./Gait_Phases.png) 34 | --------------------------------------------------------------------------------