Both sides previous revision Previous revision Next revision | Previous revision | ||
fabricademy2017:students:nuria.robles:week_9 [2018/02/15 23:40] nuriafablab_gmail.com |
fabricademy2017:students:nuria.robles:week_9 [2018/05/12 00:25] (current) nuriafablab_gmail.com |
||
---|---|---|---|
Line 18: | Line 18: | ||
Create an interactive object; if you are already experienced with coding, focus on fully integrating a microcontoller into a textile circuit. If you are new to coding, choose an example and get it working using your own sensors and actuators. | Create an interactive object; if you are already experienced with coding, focus on fully integrating a microcontoller into a textile circuit. If you are new to coding, choose an example and get it working using your own sensors and actuators. | ||
+ | |||
+ | ---- | ||
+ | |||
+ | === Digital Sensor Circuits === | ||
+ | |||
+ | A **digital sensor** is an electronic or electrochemical sensor, where [[https://en.wikipedia.org/wiki/Data_conversion|data conversion]] and data transmission are done digitally. | ||
+ | |||
+ | When a digital sensor is connected to a microcontroller, need to use a pull down or pull-up resistor. A nice tutorial about pull-up, pull-down resistor can be found [[https://learn.sparkfun.com/tutorials/pull-up-resistors|here]]. | ||
+ | |||
+ | === Reading Resistive Sensors === | ||
+ | |||
+ | The first thing I want to test is how my crochet pressure sensor acts in a voltage divider. Many sensors in the real world are simple resistive devices. A [[https://www.sparkfun.com/products/9088|photocell]] is a variable resistor, which produces a resistance proportional to the amount of light it senses. Other devices like [[https://www.sparkfun.com/products/8606?|flex sensors]], [[https://www.sparkfun.com/products/9375|force-sensitive resistors]], and [[https://www.sparkfun.com/products/250|thermistors]], are also variable resistors. | ||
+ | |||
+ | It turns out voltage is really easy for microcontrollers (those with [[https://learn.sparkfun.com/tutorials/analog-to-digital-conversion|analog-to-digital converters]] - ADC’s - at least) to measure. Resistance? Not so much. But, by adding another resistor to the resistive sensors, we can create a voltage divider. Once the output of the voltage divider is known, we can go back and calculate the resistance of the sensor. | ||
+ | |||
+ | For example, the photocell’s resistance varies between 1kΩ in the light and about 10kΩ in the dark. If we combine that with a static resistance somewhere in the middle - say 5.6kΩ, we can get a wide range out of the voltage divider they create. | ||
---- | ---- | ||
Line 39: | Line 55: | ||
{{ :fabricademy2017:students:nuria.robles:week9_etextile_wearablesii:lilypad_arduino_processing.png?nolink&600x613 }} | {{ :fabricademy2017:students:nuria.robles:week9_etextile_wearablesii:lilypad_arduino_processing.png?nolink&600x613 }} | ||
- | A video of the circuit working can be watched here: | + | Here is the arduino code : |
- | {{vimeo>255993043?large}} | + | <code> |
+ | #define OUTPUT_TEAPOT | ||
- | === === | + | #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards |
+ | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) | ||
+ | bool blinkState = false; | ||
- | === Digital Sensor Circuits === | + | // MPU control/status vars |
+ | bool dmpReady = false; // set true if DMP init was successful | ||
+ | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU | ||
+ | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) | ||
+ | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) | ||
+ | uint16_t fifoCount; // count of all bytes currently in FIFO | ||
+ | uint8_t fifoBuffer[64]; // FIFO storage buffer | ||
- | A **digital sensor** is an electronic or electrochemical sensor, where [[https://en.wikipedia.org/wiki/Data_conversion|data conversion]] and data transmission are done digitally. | + | // orientation/motion vars |
+ | Quaternion q; // [w, x, y, z] quaternion container | ||
+ | VectorInt16 aa; // [x, y, z] accel sensor measurements | ||
+ | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements | ||
+ | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements | ||
+ | VectorFloat gravity; // [x, y, z] gravity vector | ||
+ | float euler[3]; // [psi, theta, phi] Euler angle container | ||
+ | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector | ||
- | When a digital sensor is connected to a microcontroller, need to use a pull down or pull-up resistor. A nice tutorial about pull-up, pull-down resistor can be found [[https://learn.sparkfun.com/tutorials/pull-up-resistors|here]]. | + | // packet structure for InvenSense teapot demo |
+ | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, 'r', 'n' }; | ||
- | === Reading Resistive Sensors === | + | // ================================================================ |
+ | // === INTERRUPT DETECTION ROUTINE === | ||
+ | // ================================================================ | ||
- | The first thing I want to test is how my crochet pressure sensor acts in a voltage divider. Many sensors in the real world are simple resistive devices. A [[https://www.sparkfun.com/products/9088|photocell]] is a variable resistor, which produces a resistance proportional to the amount of light it senses. Other devices like [[https://www.sparkfun.com/products/8606?|flex sensors]], [[https://www.sparkfun.com/products/9375|force-sensitive resistors]], and [[https://www.sparkfun.com/products/250|thermistors]], are also variable resistors. | + | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high |
+ | void dmpDataReady() { | ||
+ | mpuInterrupt = true; | ||
+ | } | ||
- | It turns out voltage is really easy for microcontrollers (those with [[https://learn.sparkfun.com/tutorials/analog-to-digital-conversion|analog-to-digital converters]] - ADC’s - at least) to measure. Resistance? Not so much. But, by adding another resistor to the resistive sensors, we can create a voltage divider. Once the output of the voltage divider is known, we can go back and calculate the resistance of the sensor. | + | // ================================================================ |
+ | // === INITIAL SETUP === | ||
+ | // ================================================================ | ||
- | For example, the photocell’s resistance varies between 1kΩ in the light and about 10kΩ in the dark. If we combine that with a static resistance somewhere in the middle - say 5.6kΩ, we can get a wide range out of the voltage divider they create. | + | void setup() { |
+ | // join I2C bus (I2Cdev library doesn't do this automatically) | ||
+ | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE | ||
+ | Wire.begin(); | ||
+ | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties | ||
+ | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE | ||
+ | Fastwire::setup(400, true); | ||
+ | #endif | ||
+ | |||
+ | // initialize serial communication | ||
+ | // (115200 chosen because it is required for Teapot Demo output, but it's | ||
+ | // really up to you depending on your project) | ||
+ | Serial.begin(115200); | ||
+ | while (!Serial); // wait for Leonardo enumeration, others continue immediately | ||
+ | |||
+ | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino | ||
+ | // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to | ||
+ | // the baud timing being too misaligned with processor ticks. You must use | ||
+ | // 38400 or slower in these cases, or use some kind of external separate | ||
+ | // crystal solution for the UART timer. | ||
+ | |||
+ | // initialize device | ||
+ | Serial.println(F("Initializing I2C devices...")); | ||
+ | mpu.initialize(); | ||
+ | pinMode(INTERRUPT_PIN, INPUT); | ||
+ | |||
+ | // verify connection | ||
+ | Serial.println(F("Testing device connections...")); | ||
+ | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); | ||
+ | |||
+ | // wait for ready | ||
+ | Serial.println(F("nSend any character to begin DMP programming and demo: ")); | ||
+ | while (Serial.available() && Serial.read()); // empty buffer | ||
+ | while (!Serial.available()); // wait for data | ||
+ | while (Serial.available() && Serial.read()); // empty buffer again | ||
+ | |||
+ | // load and configure the DMP | ||
+ | Serial.println(F("Initializing DMP...")); | ||
+ | devStatus = mpu.dmpInitialize(); | ||
+ | |||
+ | // supply your own gyro offsets here, scaled for min sensitivity | ||
+ | mpu.setXGyroOffset(220); | ||
+ | mpu.setYGyroOffset(76); | ||
+ | mpu.setZGyroOffset(-85); | ||
+ | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip | ||
+ | |||
+ | // make sure it worked (returns 0 if so) | ||
+ | if (devStatus == 0) { | ||
+ | // turn on the DMP, now that it's ready | ||
+ | Serial.println(F("Enabling DMP...")); | ||
+ | mpu.setDMPEnabled(true); | ||
+ | |||
+ | // enable Arduino interrupt detection | ||
+ | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); | ||
+ | attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); | ||
+ | mpuIntStatus = mpu.getIntStatus(); | ||
+ | |||
+ | // set our DMP Ready flag so the main loop() function knows it's okay to use it | ||
+ | Serial.println(F("DMP ready! Waiting for first interrupt...")); | ||
+ | dmpReady = true; | ||
+ | |||
+ | // get expected DMP packet size for later comparison | ||
+ | packetSize = mpu.dmpGetFIFOPacketSize(); | ||
+ | } else { | ||
+ | // ERROR! | ||
+ | // 1 = initial memory load failed | ||
+ | // 2 = DMP configuration updates failed | ||
+ | // (if it's going to break, usually the code will be 1) | ||
+ | Serial.print(F("DMP Initialization failed (code ")); | ||
+ | Serial.print(devStatus); | ||
+ | Serial.println(F(")")); | ||
+ | } | ||
+ | |||
+ | // configure LED for output | ||
+ | pinMode(LED_PIN, OUTPUT); | ||
+ | } | ||
+ | |||
+ | // ================================================================ | ||
+ | // === MAIN PROGRAM LOOP === | ||
+ | // ================================================================ | ||
+ | |||
+ | void loop() { | ||
+ | // if programming failed, don't try to do anything | ||
+ | if (!dmpReady) return; | ||
+ | |||
+ | // wait for MPU interrupt or extra packet(s) available | ||
+ | while (!mpuInterrupt && fifoCount <packetSize) { | ||
+ | // other program behavior stuff here | ||
+ | // . | ||
+ | // . | ||
+ | // . | ||
+ | // if you are really paranoid you can frequently test in between other | ||
+ | // stuff to see if mpuInterrupt is true, and if so, "break;" from the | ||
+ | // while() loop to immediately process the MPU data | ||
+ | // . | ||
+ | // . | ||
+ | // . | ||
+ | } | ||
+ | |||
+ | // reset interrupt flag and get INT_STATUS byte | ||
+ | mpuInterrupt = false; | ||
+ | mpuIntStatus = mpu.getIntStatus(); | ||
+ | |||
+ | // get current FIFO count | ||
+ | fifoCount = mpu.getFIFOCount(); | ||
+ | |||
+ | // check for overflow (this should never happen unless our code is too inefficient) | ||
+ | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { | ||
+ | // reset so we can continue cleanly | ||
+ | mpu.resetFIFO(); | ||
+ | Serial.println(F("FIFO overflow!")); | ||
+ | |||
+ | // otherwise, check for DMP data ready interrupt (this should happen frequently) | ||
+ | } else if (mpuIntStatus & 0x02) { | ||
+ | // wait for correct available data length, should be a VERY short wait | ||
+ | while (fifoCount <packetSize) fifoCount = mpu.getFIFOCount(); | ||
+ | |||
+ | // read a packet from FIFO | ||
+ | mpu.getFIFOBytes(fifoBuffer, packetSize); | ||
+ | |||
+ | // track FIFO count here in case there is> 1 packet available | ||
+ | // (this lets us immediately read more without waiting for an interrupt) | ||
+ | fifoCount -= packetSize; | ||
+ | |||
+ | #ifdef OUTPUT_READABLE_QUATERNION | ||
+ | // display quaternion values in easy matrix form: w x y z | ||
+ | mpu.dmpGetQuaternion(&q, fifoBuffer); | ||
+ | Serial.print("quat\t"); | ||
+ | Serial.print(q.w); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(q.x); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(q.y); | ||
+ | Serial.print("\t"); | ||
+ | Serial.println(q.z); | ||
+ | #endif | ||
+ | |||
+ | #ifdef OUTPUT_READABLE_EULER | ||
+ | // display Euler angles in degrees | ||
+ | mpu.dmpGetQuaternion(&q, fifoBuffer); | ||
+ | mpu.dmpGetEuler(euler, &q); | ||
+ | Serial.print("euler\t"); | ||
+ | Serial.print(euler[0] * 180/M_PI); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(euler[1] * 180/M_PI); | ||
+ | Serial.print("\t"); | ||
+ | Serial.println(euler[2] * 180/M_PI); | ||
+ | #endif | ||
+ | |||
+ | #ifdef OUTPUT_READABLE_YAWPITCHROLL | ||
+ | // display Euler angles in degrees | ||
+ | mpu.dmpGetQuaternion(&q, fifoBuffer); | ||
+ | mpu.dmpGetGravity(&gravity, &q); | ||
+ | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); | ||
+ | Serial.print("ypr\t"); | ||
+ | Serial.print(ypr[0] * 180/M_PI); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(ypr[1] * 180/M_PI); | ||
+ | Serial.print("\t"); | ||
+ | Serial.println(ypr[2] * 180/M_PI); | ||
+ | #endif | ||
+ | |||
+ | #ifdef OUTPUT_READABLE_REALACCEL | ||
+ | // display real acceleration, adjusted to remove gravity | ||
+ | mpu.dmpGetQuaternion(&q, fifoBuffer); | ||
+ | mpu.dmpGetAccel(&aa, fifoBuffer); | ||
+ | mpu.dmpGetGravity(&gravity, &q); | ||
+ | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); | ||
+ | Serial.print("areal\t"); | ||
+ | Serial.print(aaReal.x); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(aaReal.y); | ||
+ | Serial.print("\t"); | ||
+ | Serial.println(aaReal.z); | ||
+ | #endif | ||
+ | |||
+ | #ifdef OUTPUT_READABLE_WORLDACCEL | ||
+ | // display initial world-frame acceleration, adjusted to remove gravity | ||
+ | // and rotated based on known orientation from quaternion | ||
+ | mpu.dmpGetQuaternion(&q, fifoBuffer); | ||
+ | mpu.dmpGetAccel(&aa, fifoBuffer); | ||
+ | mpu.dmpGetGravity(&gravity, &q); | ||
+ | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); | ||
+ | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); | ||
+ | Serial.print("aworld\t"); | ||
+ | Serial.print(aaWorld.x); | ||
+ | Serial.print("\t"); | ||
+ | Serial.print(aaWorld.y); | ||
+ | Serial.print("\t"); | ||
+ | Serial.println(aaWorld.z); | ||
+ | #endif | ||
+ | |||
+ | #ifdef OUTPUT_TEAPOT | ||
+ | // display quaternion values in InvenSense Teapot demo format: | ||
+ | teapotPacket[2] = fifoBuffer[0]; | ||
+ | teapotPacket[3] = fifoBuffer[1]; | ||
+ | teapotPacket[4] = fifoBuffer[4]; | ||
+ | teapotPacket[5] = fifoBuffer[5]; | ||
+ | teapotPacket[6] = fifoBuffer[8]; | ||
+ | teapotPacket[7] = fifoBuffer[9]; | ||
+ | teapotPacket[8] = fifoBuffer[12]; | ||
+ | teapotPacket[9] = fifoBuffer[13]; | ||
+ | Serial.write(teapotPacket, 14); | ||
+ | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose | ||
+ | #endif | ||
+ | |||
+ | // blink LED to indicate activity | ||
+ | blinkState = !blinkState; | ||
+ | digitalWrite(LED_PIN, blinkState); | ||
+ | } | ||
+ | } | ||
+ | </code> | ||
+ | |||
+ | And here the Processing code: | ||
+ | |||
+ | <file> | ||
+ | // I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output | ||
+ | // 6/20/2012 by Jeff Rowberg <jeff@rowberg.net> | ||
+ | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib | ||
+ | // | ||
+ | // Changelog: | ||
+ | // 2012-06-20 - initial release | ||
+ | |||
+ | /* ============================================ | ||
+ | I2Cdev device library code is placed under the MIT license | ||
+ | Copyright (c) 2012 Jeff Rowberg | ||
+ | |||
+ | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
+ | of this software and associated documentation files (the "Software"), to deal | ||
+ | in the Software without restriction, including without limitation the rights | ||
+ | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
+ | copies of the Software, and to permit persons to whom the Software is | ||
+ | furnished to do so, subject to the following conditions: | ||
+ | |||
+ | The above copyright notice and this permission notice shall be included in | ||
+ | all copies or substantial portions of the Software. | ||
+ | |||
+ | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
+ | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
+ | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
+ | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
+ | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
+ | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
+ | THE SOFTWARE. | ||
+ | =============================================== | ||
+ | */ | ||
+ | |||
+ | import processing.serial.*; | ||
+ | import processing.opengl.*; | ||
+ | import toxi.geom.*; | ||
+ | import toxi.processing.*; | ||
+ | import processing.video.*; | ||
+ | |||
+ | // NOTE: requires ToxicLibs to be installed in order to run properly. | ||
+ | // 1. Download from http://toxiclibs.org/downloads | ||
+ | // 2. Extract into [userdir]/Processing/libraries | ||
+ | // (location may be different on Mac/Linux) | ||
+ | // 3. Run and bask in awesomeness | ||
+ | |||
+ | ToxiclibsSupport gfx; | ||
+ | |||
+ | Serial port; // The serial port | ||
+ | char[] teapotPacket = new char[14]; // InvenSense Teapot packet | ||
+ | int serialCount = 0; // current packet byte position | ||
+ | int synced = 0; | ||
+ | int interval = 0; | ||
+ | |||
+ | float[] q = new float[4]; | ||
+ | Quaternion quat = new Quaternion(1, 0, 0, 0); | ||
+ | |||
+ | float[] gravity = new float[3]; | ||
+ | float[] euler = new float[3]; | ||
+ | float[] ypr = new float[3]; | ||
+ | |||
+ | PShape saturn; //prueba de nuria para cargar objeto 3d | ||
+ | PImage grid; //imagen de fondo | ||
+ | Movie movie; //video fondo | ||
+ | |||
+ | //PShape star; //prueba de nuria para cargar objeto 3d | ||
+ | void setup() { | ||
+ | // 300px square viewport using OpenGL rendering | ||
+ | //size(300, 300, OPENGL); // original | ||
+ | size(800, 800, OPENGL);//comentado por nuria: mayor tamaño | ||
+ | saturn = loadShape("planet_final.obj");//codigo nuria | ||
+ | //saturn.disableStyle(); | ||
+ | //fill (225,128,0); | ||
+ | //shape (saturn); | ||
+ | |||
+ | //Borja | ||
+ | // | ||
+ | //saturn.setStroke(true); //Lineas Poligonales | ||
+ | //saturn.setStroke(color(255,0,0)); //Color de la Linea Rojo | ||
+ | //saturn.setStrokeWeight(2); //Grosor de la Linea | ||
+ | |||
+ | grid = loadImage("DomeGrid_2k.png"); // cargamos foto de fondo | ||
+ | movie = new Movie(this, "transit.mov"); //poner un video | ||
+ | movie.loop(); | ||
+ | //star = loadShape("star.obj");//codigo nuria | ||
+ | gfx = new ToxiclibsSupport(this); | ||
+ | |||
+ | // setup lights and antialiasing | ||
+ | lights(); | ||
+ | smooth(); | ||
+ | |||
+ | // display serial port list for debugging/clarity | ||
+ | println(Serial.list()); | ||
+ | |||
+ | // get the first available port (use EITHER this OR the specific port code below) | ||
+ | //String portName = Serial.list()[0]; | ||
+ | |||
+ | // get a specific serial port (use EITHER this OR the first-available code above) | ||
+ | //String portName = "/dev/cu.usbmodem1411"; //nuria: arduino | ||
+ | String portName = "/dev/tty.usbmodem14121"; | ||
+ | //String portName = "/dev/tty.usbserial-A50285BI"; //nuria: ftdi rojo | ||
+ | |||
+ | // open the serial port | ||
+ | port = new Serial(this, portName, 115200); | ||
+ | |||
+ | // send single character to trigger DMP init/start | ||
+ | // (expected by MPU6050_DMP6 example Arduino sketch) | ||
+ | port.write('r'); | ||
+ | } | ||
+ | |||
+ | void movieEvent(Movie m) { | ||
+ | m.read(); | ||
+ | } | ||
+ | |||
+ | void draw() { | ||
+ | if (millis() - interval> 1000) { | ||
+ | port.write('r'); | ||
+ | interval = millis(); | ||
+ | } | ||
+ | |||
+ | //background(0,255,35); //fondo verde | ||
+ | image(movie, 0, 0, width, height); //VIDEO FONDO | ||
+ | //image(grid, 0,0, width, height); //FOTO de FONDO | ||
+ | pushMatrix(); | ||
+ | |||
+ | translate(width / 2, height / 2, 150); //Nuria la ultima coordenada es para traerlo por delante del video | ||
+ | lights(); | ||
+ | |||
+ | float[] axis = quat.toAxisAngle(); | ||
+ | rotate(axis[0], -axis[1], axis[3], axis[2]); | ||
+ | noStroke();//nuria: de marta | ||
+ | scale(10,10,10);//nuria: para agrandar | ||
+ | |||
+ | fill(255); //nuria: para cambiar de color | ||
+ | shape(saturn); | ||
+ | |||
+ | popMatrix(); | ||
+ | } | ||
+ | |||
+ | void serialEvent(Serial port) { | ||
+ | interval = millis(); | ||
+ | while (port.available()> 0) { | ||
+ | int ch = port.read(); | ||
+ | |||
+ | if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed | ||
+ | synced = 1; | ||
+ | print ((char)ch); | ||
+ | |||
+ | if ((serialCount == 1 && ch != 2) | ||
+ | || (serialCount == 12 && ch != '\r') | ||
+ | || (serialCount == 13 && ch != '\n')) { | ||
+ | serialCount = 0; | ||
+ | synced = 0; | ||
+ | return; | ||
+ | } | ||
+ | |||
+ | if (serialCount> 0 || ch == '$') { | ||
+ | teapotPacket[serialCount++] = (char)ch; | ||
+ | if (serialCount == 14) { | ||
+ | serialCount = 0; // restart packet byte position | ||
+ | |||
+ | // get quaternion from data packet | ||
+ | q[0] = ((teapotPacket[2] <<8) | teapotPacket[3]) / 16384.0f; | ||
+ | q[1] = ((teapotPacket[4] <<8) | teapotPacket[5]) / 16384.0f; | ||
+ | q[2] = ((teapotPacket[6] <<8) | teapotPacket[7]) / 16384.0f; | ||
+ | q[3] = ((teapotPacket[8] <<8) | teapotPacket[9]) / 16384.0f; | ||
+ | for (int i = 0; i <4; i++) if (q[i]>= 2) q[i] = -4 + q[i]; | ||
+ | |||
+ | // set our toxilibs quaternion to new data | ||
+ | quat.set(q[0], q[1], q[2], q[3]); | ||
+ | |||
+ | /* | ||
+ | // below calculations unnecessary for orientation only using toxilibs | ||
+ | |||
+ | // calculate gravity vector | ||
+ | gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); | ||
+ | gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); | ||
+ | gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; | ||
+ | |||
+ | // calculate Euler angles | ||
+ | euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); | ||
+ | euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); | ||
+ | euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); | ||
+ | |||
+ | // calculate yaw/pitch/roll angles | ||
+ | ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); | ||
+ | ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); | ||
+ | ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); | ||
+ | |||
+ | // output various components for debugging | ||
+ | //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); | ||
+ | //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); | ||
+ | //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); | ||
+ | */ | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | |||
+ | void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { | ||
+ | float angle = 0; | ||
+ | float angleIncrement = TWO_PI / sides; | ||
+ | beginShape(QUAD_STRIP); | ||
+ | for (int i = 0; i <sides + 1; ++i) { | ||
+ | vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); | ||
+ | vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); | ||
+ | angle += angleIncrement; | ||
+ | } | ||
+ | endShape(); | ||
+ | |||
+ | // If it is not a cone, draw the circular top cap | ||
+ | if (topRadius != 0) { | ||
+ | angle = 0; | ||
+ | beginShape(TRIANGLE_FAN); | ||
+ | |||
+ | // Center point | ||
+ | vertex(0, 0, 0); | ||
+ | for (int i = 0; i <sides + 1; i++) { | ||
+ | vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); | ||
+ | angle += angleIncrement; | ||
+ | } | ||
+ | endShape(); | ||
+ | } | ||
+ | |||
+ | // If it is not a cone, draw the circular bottom cap | ||
+ | if (bottomRadius != 0) { | ||
+ | angle = 0; | ||
+ | beginShape(TRIANGLE_FAN); | ||
+ | |||
+ | // Center point | ||
+ | vertex(0, tall, 0); | ||
+ | for (int i = 0; i <sides + 1; i++) { | ||
+ | vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); | ||
+ | angle += angleIncrement; | ||
+ | } | ||
+ | endShape(); | ||
+ | } | ||
+ | } | ||
+ | </file> | ||
+ | |||
+ | A video of the circuit working can be watched here: | ||
+ | |||
+ | {{vimeo>255993043?large}} | ||
+ | |||
+ | === === | ||
+ | |||
+ | === === | ||