stm32 arduino added

stm32 arduino added

error: pathspec 'stm32/ws2812_ch16_spi.ino' did not match any file(s) known to git

--- stm32/ws2812_ch16_spi.ino
+++ stm32/ws2812_ch16_spi.ino
@@ -0,0 +1,397 @@
+#include "libmaple/gpio.h"
+#include "libmaple/dma.h"
+#include "libmaple/timer.h"
+#include <SPI.h>
+
+// soft reset stuff
+#define SCB_AIRCR ((volatile uint32*) (0xE000ED00 + 0x0C))
+#define SCB_AIRCR_SYSRESETREQ (1 << 2)
+#define SCB_AIRCR_RESET ((0x05FA0000) | SCB_AIRCR_SYSRESETREQ)
+
+#define pinLED PC13
+#define WS2812_DEADPERIOD 19
+
+#define MIN_COLS 1
+#define MAX_COLS 255
+
+#define ROWS 16
+
+#define HANDSHAKE 0xaa
+
+typedef union {
+  uint16_t val16;
+  struct bytes_t {
+    unsigned msb : 8;
+    unsigned lsb : 8;
+  } bytes;
+} val16_t;
+
+uint16_t WS2812_IO_H = 0xFFFF;
+uint16_t WS2812_IO_L = 0x0000;
+
+volatile uint8_t WS2812_TC = 1;
+volatile uint8_t TIM4_overflows = 0;
+
+/* WS2812 framebuffer */
+uint16_t *ledbuf;
+uint16_t *WS2812_rxbuffer;
+uint16_t *WS2812_txbuffer;
+
+volatile uint8_t dma_rx_irq_full_complete = 0;
+volatile uint8_t dma_rx_irq_half_complete = 0;
+
+//uint16_t cols;
+val16_t cols;
+uint32_t led_bufsize = 0;
+
+
+void DMA1_Channel2_IRQHandler(void){
+  uint32_t dma_isr = dma_get_isr_bits(DMA1, DMA_CH2);
+  if (dma_isr&DMA_ISR_HTIF1) {
+    dma_rx_irq_half_complete = 1;
+  }
+  if (dma_isr&DMA_ISR_TCIF1) {
+    dma_rx_irq_full_complete = 1;
+    //WS2812_txbuffer = ledbuf;
+  }
+  dma_clear_isr_bits(DMA1, DMA_CH2);
+  digitalWrite(pinLED, LOW);
+}
+
+/* DMA1 Channel7 Interrupt Handler gets executed once the complete framebuffer has been transmitted to the LEDs */
+void DMA1_Channel5_IRQHandler(void)
+{
+  // clear DMA7 transfer complete interrupt flag
+  dma_clear_isr_bits(DMA1, DMA_CH5);
+  // enable TIM4 Update interrupt to append 50us dead period
+  timer_enable_irq(TIMER4, TIMER_UPDATE_INTERRUPT);
+  // disable the DMA channels
+  dma_disable(DMA1, DMA_CH7);
+  dma_disable(DMA1, DMA_CH4);
+  dma_disable(DMA1, DMA_CH5);
+  // IMPORTANT: disable the DMA requests, too!
+  timer_dma_disable_req(TIMER4, 1);
+  timer_dma_disable_req(TIMER4, 2);
+  timer_dma_disable_req(TIMER4, 0); /* TIM_DMA_Update */
+}
+
+/* TIM4 Interrupt Handler gets executed on every TIM4 Update if enabled */
+void TIM4_IRQHandler(void)
+{
+  // Clear TIM4 Interrupt Flag
+  TIMER4->regs.gen->SR &= ~TIMER_SR_UIF;
+  /* check if certain number of overflows has occured yet
+   * this ISR is used to guarantee a 50us dead time on the data lines
+   * before another frame is transmitted */
+  if (TIM4_overflows < (uint8_t)WS2812_DEADPERIOD)
+  {
+    // count the number of occured overflows
+    TIM4_overflows++;
+  }
+  else
+  {
+    // clear the number of overflows
+    TIM4_overflows = 0;
+    // stop TIM4 now because dead period has been reached
+    timer_pause(TIMER4);
+    /* disable the TIM4 Update interrupt again
+     * so it doesn't occur while transmitting data */
+    timer_disable_irq(TIMER4, TIMER_UPDATE_INTERRUPT);
+    // finally indicate that the data frame has been transmitted
+    //WS2812_rxbuffer = ledbuf;
+    WS2812_TC = 1;
+  }
+}
+
+void GPIO_init(void)
+{
+  // GPIOB Periph clock enable
+  rcc_clk_enable(RCC_GPIOB);
+  // GPIOB pins WS2812 data outputs
+  gpio_set_mode(GPIOB, 0, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 1, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 2, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 3, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 4, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 5, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 6, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 7, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 8, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 9, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 10, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 11, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 12, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 13, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 14, GPIO_OUTPUT_PP);
+  gpio_set_mode(GPIOB, 15, GPIO_OUTPUT_PP);
+}
+
+void TIM4_init(void) {
+  uint32_t SystemCoreClock = 72000000;
+  uint16_t prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
+  rcc_clk_enable(RCC_TIMER4);
+  /* Time base configuration */
+  timer_pause(TIMER4);
+  timer_set_prescaler(TIMER4, prescalerValue);
+  timer_set_reload(TIMER4, 29); // 800kHz
+
+  /* Timing Mode configuration: Channel 1 */
+  timer_set_mode(TIMER4, 2, TIMER_OUTPUT_COMPARE);
+  timer_set_compare(TIMER4, 2, 8);
+  timer_oc_set_mode(TIMER4, 2, TIMER_OC_MODE_FROZEN, ~TIMER_OC_PE);
+
+  /* Timing Mode configuration: Channel 2 */
+  timer_set_mode(TIMER4, 3, TIMER_OUTPUT_COMPARE);
+  timer_set_compare(TIMER4, 3, 17);
+  timer_oc_set_mode(TIMER4, 3, TIMER_OC_MODE_PWM_1, ~TIMER_OC_PE);
+  //timer_resume(TIMER4);
+
+
+  timer_attach_interrupt(TIMER4, TIMER_UPDATE_INTERRUPT, TIM4_IRQHandler);
+  /* configure TIM4 interrupt */
+  nvic_irq_set_priority(NVIC_TIMER4, 3);
+  nvic_irq_enable(NVIC_TIMER4);
+}
+
+void DMA_init(void) {
+  dma_init(DMA1);
+  dma_setup_transfer( DMA1,
+                      DMA_CH7,
+                      (volatile void*) &(GPIOB->regs->ODR),
+                      DMA_SIZE_16BITS,
+                      (volatile void*) &(WS2812_IO_H),
+                      DMA_SIZE_16BITS,
+                      DMA_FROM_MEM
+                     );
+  dma_set_priority(DMA1, DMA_CH7, DMA_PRIORITY_HIGH);
+
+  dma_setup_transfer( DMA1,
+                      DMA_CH4,
+                      (volatile void*) &(GPIOB->regs->ODR),
+                      DMA_SIZE_16BITS,
+                      (volatile void*) WS2812_txbuffer,
+                      DMA_SIZE_16BITS,
+                      DMA_FROM_MEM | DMA_MINC_MODE
+                    );
+  dma_set_priority(DMA1, DMA_CH4, DMA_PRIORITY_HIGH);
+
+  dma_setup_transfer( DMA1,
+                      DMA_CH5,
+                      (volatile void*) &(GPIOB->regs->ODR),
+                      DMA_SIZE_16BITS,
+                      (volatile void*) &(WS2812_IO_L),
+                      DMA_SIZE_16BITS,
+                      DMA_FROM_MEM | DMA_TRNS_CMPLT
+                    );
+  dma_set_priority(DMA1, DMA_CH5, DMA_PRIORITY_HIGH);
+
+
+  /* configure DMA1 Channel7 interrupt */
+  nvic_irq_set_priority(NVIC_DMA_CH5, 1);
+  nvic_irq_enable(NVIC_DMA_CH5);
+  dma_attach_interrupt(DMA1, DMA_CH5, DMA1_Channel5_IRQHandler);
+  /* enable DMA1 Channel7 transfer complete interrupt */
+}
+
+void ledSend(uint32_t size){
+  // transmission complete flag, indicate that transmission is taking place
+  WS2812_TC = 0;
+  // clear all relevant DMA flags
+  dma_clear_isr_bits(DMA1, DMA_CH7);
+  dma_clear_isr_bits(DMA1, DMA_CH4);
+  dma_clear_isr_bits(DMA1, DMA_CH5);
+
+  dma_disable(DMA1, DMA_CH7);
+  dma_disable(DMA1, DMA_CH4);
+  dma_disable(DMA1, DMA_CH5);
+  // IMPORTANT: disable the DMA requests, too!
+
+  timer_dma_disable_req(TIMER4, 3);
+  timer_dma_disable_req(TIMER4, 2);
+  timer_dma_disable_req(TIMER4, 0);
+
+
+  // configure the number of bytes to be transferred by the DMA controller
+  //dma_set_mem_addr(DMA1, DMA_CH4, WS2812_rxbuffer);
+  dma_set_num_transfers(DMA1, DMA_CH7, size);
+  dma_set_num_transfers(DMA1, DMA_CH4, size);
+  dma_set_num_transfers(DMA1, DMA_CH5, size);
+
+  // clear all TIM4 flags
+  TIMER4->regs.gen->SR = 0;
+
+  // enable the corresponding DMA channels
+  dma_enable(DMA1, DMA_CH7);
+  dma_enable(DMA1, DMA_CH4);
+  dma_enable(DMA1, DMA_CH5);
+  // IMPORTANT: enable the TIM4 DMA requests AFTER enabling the DMA channels!
+  timer_dma_enable_req(TIMER4, 3);
+  timer_dma_enable_req(TIMER4, 2);
+  timer_dma_enable_req(TIMER4, 0); /* TIM_DMA_Update */
+
+  // preload counter with 29 so TIM4 generates UEV directly to start DMA transfer
+  timer_set_count(TIMER4, 29);
+
+  // start TIM4
+  timer_resume(TIMER4);
+}
+
+void ledSetPixel(uint8_t row, uint16_t column, uint8_t red, uint8_t green, uint8_t blue, uint16_t *pbuf){
+uint8_t i;
+uint32_t offset=column*3*8;
+uint32_t pos_g;
+uint32_t pos_r;
+uint32_t pos_b;
+uint16_t delmask = ~(0x0001<<row);
+
+  for (i = 0; i < 8; i++){
+    pos_g=offset+i;
+    pos_r=pos_g+8;
+    pos_b=pos_r+8;
+    // clear the data for pixel
+    pbuf[pos_g] &= delmask;
+    pbuf[pos_r] &= delmask;
+    pbuf[pos_b] &= delmask;
+    // write new data for pixel
+    pbuf[pos_g] |= (((((uint16_t)green<<(i+ROWS-8)) & (0x0001<<ROWS-1))>>(ROWS-1))<<row);
+    pbuf[pos_r] |= (((((uint16_t)red<<(i+ROWS-8)) & (0x0001<<ROWS-1))>>(ROWS-1))<<row);
+    pbuf[pos_b] |= (((((uint16_t)blue<<(i+ROWS-8)) & (0x0001<<ROWS-1))>>(ROWS-1))<<row);
+  }
+}
+
+void ledWakeUpSequence(void){
+uint32_t row;
+uint32_t col;
+uint8_t val;
+  // fade in
+  val=0;
+    while(val!=255){
+    for(row=0;row<ROWS;row++){
+      for(col=0;col<cols.val16;col++){
+        ledSetPixel(row, col, val, val, val, ledbuf);
+      }
+    }
+    val++;
+    ledSend(led_bufsize);
+    while(!WS2812_TC);
+
+    }
+
+    // fade out
+  val=255;
+    while(val!=0){
+      val--;
+    for(row=0;row<ROWS;row++){
+      for(col=0;col<cols.val16;col++){
+        ledSetPixel(row, col, val, val, val, ledbuf);
+      }
+    }
+    ledSend(led_bufsize);
+    while(!WS2812_TC);
+    }
+}
+
+void setup() {
+int col,row;
+uint8_t initdata;
+  pinMode(pinLED, OUTPUT);
+
+  Serial.begin(115200);
+  delay(100);
+
+
+  SPI.setModule(1);
+  SPI.setDataSize(8);
+  SPI.beginSlave();
+
+  Serial.println(F("OK:  Waiting for Handshake..."));
+
+  initdata = SPI.read();
+  cols.bytes.msb= SPI.read();
+  cols.bytes.lsb= SPI.read();
+  if(initdata != HANDSHAKE){
+    Serial.print(F("ERR: Magic number HANDSHAKE not received (0x"));
+    Serial.print(initdata,HEX);
+    Serial.print(F(").\n"));
+  }
+  else{
+    Serial.print(F("OK:  Magic number received (0x"));
+    Serial.print(initdata,HEX);
+    Serial.print(F(").\n"));
+  }
+
+  if (!((cols.val16 >= MIN_COLS) && (cols.val16 <= MAX_COLS))) {
+    Serial.println(F("ERR: #cols not within range. Setting to cols=1 failsafe mode"));
+    cols.val16=1;
+  }
+  else{
+    Serial.print(F("OK:  Number of cols received ("));
+    Serial.print(cols.val16,DEC);
+    Serial.print(F(").\n"));
+  }
+
+
+  led_bufsize=cols.val16*8*3;
+  ledbuf=(uint16_t *)malloc(led_bufsize);
+  if (!ledbuf){
+      Serial.println(F("ERR: Allocating memory buffer failed"));
+  }
+  else{
+    Serial.print(F("OK:  "));
+    Serial.print(led_bufsize*2,DEC);
+    Serial.println(F(" bytes memory buffer allocated."));
+  }
+
+  SPI.write((unsigned int)HANDSHAKE);
+  SPI.read();
+  SPI.write((unsigned int)cols.bytes.msb);
+  SPI.read(); //dummy read
+  SPI.write((unsigned int)cols.bytes.lsb);
+  SPI.read(); //dummy read
+
+  WS2812_txbuffer = ledbuf;
+
+  GPIO_init();
+  DMA_init();
+  TIM4_init();
+  WS2812_TC=0;
+
+  int ret;
+  //memset(ledbuf,0,led_bufsize);
+
+
+  dma_clear_isr_bits(DMA1, DMA_CH2);
+  spi_rx_dma_enable(SPI1);
+  dma_disable(DMA1, DMA_CH2); // Disable the DMA tube.
+  dma_tube_config rx_tube_cfg = {
+    &(SPI1->regs->DR),    // data source address
+    DMA_SIZE_8BITS,    // source transfer size
+    (uint8_t*)ledbuf,               // data destination address
+    DMA_SIZE_8BITS,    // destination transfer size
+    led_bufsize*2,      // nr. of data to transfer
+    // tube flags: auto increment dest addr, circular rx_buffer, set tube full IRQ, very high prio:
+    ( DMA_CFG_DST_INC | DMA_CFG_CIRC | DMA_CFG_HALF_CMPLT_IE | DMA_CFG_CMPLT_IE | DMA_CCR_PL_VERY_HIGH ),
+    0,  // unused
+    DMA_REQ_SRC_SPI1_RX,  // Hardware DMA request source
+  };
+  ret = dma_tube_cfg(DMA1, DMA_CH2, &rx_tube_cfg);  // SPI1 Rx channel is nr. 2
+
+  dma_attach_interrupt(DMA1, DMA_CH2, DMA1_Channel2_IRQHandler);  // attach an interrupt handler.
+  dma_enable(DMA1, DMA_CH2);  // Enable the DMA tube. It will now begin serving requests.
+
+  if(ret!=0){
+    Serial.println(F("ERR: dma_ch2 spi rx init error"));
+  }
+  else{
+    digitalWrite(pinLED, HIGH);
+    Serial.println(F("OK:  Init done."));
+  }
+
+}
+
+void loop() {
+  while(dma_rx_irq_half_complete==0) digitalWrite(pinLED, HIGH);
+  dma_rx_irq_half_complete=0;
+  ledSend(led_bufsize);
+  while(!WS2812_TC);
+}


Initial Commit

Initial Commit

error: pathspec 'SensorShirt/SensorShirt.ino' did not match any file(s) known to git

--- SensorShirt/SensorShirt.ino
+++ SensorShirt/SensorShirt.ino
@@ -0,0 +1,14 @@
+/*
+  Sensor Shirt
+  Kylie Dale and Michael Guida
+ */
+
+void setup() {
+  Serial.begin(9600);
+}
+
+void loop() {
+  int sensorValue = analogRead(A0);
+  Serial.println(sensorValue);
+  delay(300);
+}


Left Hand

Left Hand

Initial Commit for Arduino C Code : Arduino Pro Mini

error: pathspec 'BitBangingMpu6050/LeftHand.ino' did not match any file(s) known to git

--- BitBangingMpu6050/LeftHand.ino
+++ BitBangingMpu6050/LeftHand.ino
@@ -0,0 +1,2337 @@
+//Flick Project
+#define SDA_PORT PORTD
+#define SDA_PIN 4
+#define SCL_PORT PORTD
+#define SCL_PIN 5
+
+#define I2C_TIMEOUT 100
+#define I2C_FASTMODE 1
+
+#include "SoftWire.h"
+
+#include <SoftwareSerial.h>
+SoftwareSerial mySerial(2, 3);
+
+SoftWire Wire = SoftWire();
+
+
+// The name of the sensor is "MPU-6050".
+// For program code, I omit the '-',
+// therefor I use the name "MPU6050....".
+
+
+// Register names according to the datasheet.
+// According to the InvenSense document
+// "MPU-6000 and MPU-6050 Register Map
+// and Descriptions Revision 3.2", there are no registers
+// at 0x02 ... 0x18, but according other information
+// the registers in that unknown area are for gain
+// and offsets.
+//
+#define MPU6050_AUX_VDDIO          0x01   // R/W
+#define MPU6050_SMPLRT_DIV         0x19   // R/W
+#define MPU6050_CONFIG             0x1A   // R/W
+#define MPU6050_GYRO_CONFIG        0x1B   // R/W
+#define MPU6050_ACCEL_CONFIG       0x1C   // R/W
+#define MPU6050_FF_THR             0x1D   // R/W
+#define MPU6050_FF_DUR             0x1E   // R/W
+#define MPU6050_MOT_THR            0x1F   // R/W
+#define MPU6050_MOT_DUR            0x20   // R/W
+#define MPU6050_ZRMOT_THR          0x21   // R/W
+#define MPU6050_ZRMOT_DUR          0x22   // R/W
+#define MPU6050_FIFO_EN            0x23   // R/W
+#define MPU6050_I2C_MST_CTRL       0x24   // R/W
+#define MPU6050_I2C_SLV0_ADDR      0x25   // R/W
+#define MPU6050_I2C_SLV0_REG       0x26   // R/W
+#define MPU6050_I2C_SLV0_CTRL      0x27   // R/W
+#define MPU6050_I2C_SLV1_ADDR      0x28   // R/W
+#define MPU6050_I2C_SLV1_REG       0x29   // R/W
+#define MPU6050_I2C_SLV1_CTRL      0x2A   // R/W
+#define MPU6050_I2C_SLV2_ADDR      0x2B   // R/W
+#define MPU6050_I2C_SLV2_REG       0x2C   // R/W
+#define MPU6050_I2C_SLV2_CTRL      0x2D   // R/W
+#define MPU6050_I2C_SLV3_ADDR      0x2E   // R/W
+#define MPU6050_I2C_SLV3_REG       0x2F   // R/W
+#define MPU6050_I2C_SLV3_CTRL      0x30   // R/W
+#define MPU6050_I2C_SLV4_ADDR      0x31   // R/W
+#define MPU6050_I2C_SLV4_REG       0x32   // R/W
+#define MPU6050_I2C_SLV4_DO        0x33   // R/W
+#define MPU6050_I2C_SLV4_CTRL      0x34   // R/W
+#define MPU6050_I2C_SLV4_DI        0x35   // R
+#define MPU6050_I2C_MST_STATUS     0x36   // R
+#define MPU6050_INT_PIN_CFG        0x37   // R/W
+#define MPU6050_INT_ENABLE         0x38   // R/W
+#define MPU6050_INT_STATUS         0x3A   // R
+#define MPU6050_ACCEL_XOUT_H       0x3B   // R
+#define MPU6050_ACCEL_XOUT_L       0x3C   // R
+#define MPU6050_ACCEL_YOUT_H       0x3D   // R
+#define MPU6050_ACCEL_YOUT_L       0x3E   // R
+#define MPU6050_ACCEL_ZOUT_H       0x3F   // R
+#define MPU6050_ACCEL_ZOUT_L       0x40   // R
+#define MPU6050_TEMP_OUT_H         0x41   // R
+#define MPU6050_TEMP_OUT_L         0x42   // R
+#define MPU6050_GYRO_XOUT_H        0x43   // R
+#define MPU6050_GYRO_XOUT_L        0x44   // R
+#define MPU6050_GYRO_YOUT_H        0x45   // R
+#define MPU6050_GYRO_YOUT_L        0x46   // R
+#define MPU6050_GYRO_ZOUT_H        0x47   // R
+#define MPU6050_GYRO_ZOUT_L        0x48   // R
+#define MPU6050_EXT_SENS_DATA_00   0x49   // R
+#define MPU6050_EXT_SENS_DATA_01   0x4A   // R
+#define MPU6050_EXT_SENS_DATA_02   0x4B   // R
+#define MPU6050_EXT_SENS_DATA_03   0x4C   // R
+#define MPU6050_EXT_SENS_DATA_04   0x4D   // R
+#define MPU6050_EXT_SENS_DATA_05   0x4E   // R
+#define MPU6050_EXT_SENS_DATA_06   0x4F   // R
+#define MPU6050_EXT_SENS_DATA_07   0x50   // R
+#define MPU6050_EXT_SENS_DATA_08   0x51   // R
+#define MPU6050_EXT_SENS_DATA_09   0x52   // R
+#define MPU6050_EXT_SENS_DATA_10   0x53   // R
+#define MPU6050_EXT_SENS_DATA_11   0x54   // R
+#define MPU6050_EXT_SENS_DATA_12   0x55   // R
+#define MPU6050_EXT_SENS_DATA_13   0x56   // R
+#define MPU6050_EXT_SENS_DATA_14   0x57   // R
+#define MPU6050_EXT_SENS_DATA_15   0x58   // R
+#define MPU6050_EXT_SENS_DATA_16   0x59   // R
+#define MPU6050_EXT_SENS_DATA_17   0x5A   // R
+#define MPU6050_EXT_SENS_DATA_18   0x5B   // R
+#define MPU6050_EXT_SENS_DATA_19   0x5C   // R
+#define MPU6050_EXT_SENS_DATA_20   0x5D   // R
+#define MPU6050_EXT_SENS_DATA_21   0x5E   // R
+#define MPU6050_EXT_SENS_DATA_22   0x5F   // R
+#define MPU6050_EXT_SENS_DATA_23   0x60   // R
+#define MPU6050_MOT_DETECT_STATUS  0x61   // R
+#define MPU6050_I2C_SLV0_DO        0x63   // R/W
+#define MPU6050_I2C_SLV1_DO        0x64   // R/W
+#define MPU6050_I2C_SLV2_DO        0x65   // R/W
+#define MPU6050_I2C_SLV3_DO        0x66   // R/W
+#define MPU6050_I2C_MST_DELAY_CTRL 0x67   // R/W
+#define MPU6050_SIGNAL_PATH_RESET  0x68   // R/W
+#define MPU6050_MOT_DETECT_CTRL    0x69   // R/W
+#define MPU6050_USER_CTRL          0x6A   // R/W
+#define MPU6050_PWR_MGMT_1         0x6B   // R/W
+#define MPU6050_PWR_MGMT_2         0x6C   // R/W
+#define MPU6050_FIFO_COUNTH        0x72   // R/W
+#define MPU6050_FIFO_COUNTL        0x73   // R/W
+#define MPU6050_FIFO_R_W           0x74   // R/W
+#define MPU6050_WHO_AM_I           0x75   // R
+
+
+// Defines for the bits, to be able to change
+// between bit number and binary definition.
+// By using the bit number, programming the sensor
+// is like programming the AVR microcontroller.
+// But instead of using "(1<<X)", or "_BV(X)",
+// the Arduino "bit(X)" is used.
+#define MPU6050_D0 0
+#define MPU6050_D1 1
+#define MPU6050_D2 2
+#define MPU6050_D3 3
+#define MPU6050_D4 4
+#define MPU6050_D5 5
+#define MPU6050_D6 6
+#define MPU6050_D7 7
+
+// AUX_VDDIO Register
+#define MPU6050_AUX_VDDIO MPU6050_D7  // I2C high: 1=VDD, 0=VLOGIC
+
+// CONFIG Register
+// DLPF is Digital Low Pass Filter for both gyro and accelerometers.
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_DLPF_CFG0     MPU6050_D0
+#define MPU6050_DLPF_CFG1     MPU6050_D1
+#define MPU6050_DLPF_CFG2     MPU6050_D2
+#define MPU6050_EXT_SYNC_SET0 MPU6050_D3
+#define MPU6050_EXT_SYNC_SET1 MPU6050_D4
+#define MPU6050_EXT_SYNC_SET2 MPU6050_D5
+
+// Combined definitions for the EXT_SYNC_SET values
+#define MPU6050_EXT_SYNC_SET_0 (0)
+#define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0))
+#define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1))
+#define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
+#define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2))
+#define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0))
+#define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1))
+#define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
+
+// Alternative names for the combined definitions.
+#define MPU6050_EXT_SYNC_DISABLED     MPU6050_EXT_SYNC_SET_0
+#define MPU6050_EXT_SYNC_TEMP_OUT_L   MPU6050_EXT_SYNC_SET_1
+#define MPU6050_EXT_SYNC_GYRO_XOUT_L  MPU6050_EXT_SYNC_SET_2
+#define MPU6050_EXT_SYNC_GYRO_YOUT_L  MPU6050_EXT_SYNC_SET_3
+#define MPU6050_EXT_SYNC_GYRO_ZOUT_L  MPU6050_EXT_SYNC_SET_4
+#define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5
+#define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6
+#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7
+
+// Combined definitions for the DLPF_CFG values
+#define MPU6050_DLPF_CFG_0 (0)
+#define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0))
+#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1))
+#define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
+#define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2))
+#define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0))
+#define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1))
+#define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
+
+// Alternative names for the combined definitions
+// This name uses the bandwidth (Hz) for the accelometer,
+// for the gyro the bandwidth is almost the same.
+#define MPU6050_DLPF_260HZ    MPU6050_DLPF_CFG_0
+#define MPU6050_DLPF_184HZ    MPU6050_DLPF_CFG_1
+#define MPU6050_DLPF_94HZ     MPU6050_DLPF_CFG_2
+#define MPU6050_DLPF_44HZ     MPU6050_DLPF_CFG_3
+#define MPU6050_DLPF_21HZ     MPU6050_DLPF_CFG_4
+#define MPU6050_DLPF_10HZ     MPU6050_DLPF_CFG_5
+#define MPU6050_DLPF_5HZ      MPU6050_DLPF_CFG_6
+#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7
+
+// GYRO_CONFIG Register
+// The XG_ST, YG_ST, ZG_ST are bits for selftest.
+// The FS_SEL sets the range for the gyro.
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_FS_SEL0 MPU6050_D3
+#define MPU6050_FS_SEL1 MPU6050_D4
+#define MPU6050_ZG_ST   MPU6050_D5
+#define MPU6050_YG_ST   MPU6050_D6
+#define MPU6050_XG_ST   MPU6050_D7
+
+// Combined definitions for the FS_SEL values
+#define MPU6050_FS_SEL_0 (0)
+#define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0))
+#define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1))
+#define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0))
+
+// Alternative names for the combined definitions
+// The name uses the range in degrees per second.
+#define MPU6050_FS_SEL_250  MPU6050_FS_SEL_0
+#define MPU6050_FS_SEL_500  MPU6050_FS_SEL_1
+#define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2
+#define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3
+
+// ACCEL_CONFIG Register
+// The XA_ST, YA_ST, ZA_ST are bits for selftest.
+// The AFS_SEL sets the range for the accelerometer.
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_ACCEL_HPF0 MPU6050_D0
+#define MPU6050_ACCEL_HPF1 MPU6050_D1
+#define MPU6050_ACCEL_HPF2 MPU6050_D2
+#define MPU6050_AFS_SEL0   MPU6050_D3
+#define MPU6050_AFS_SEL1   MPU6050_D4
+#define MPU6050_ZA_ST      MPU6050_D5
+#define MPU6050_YA_ST      MPU6050_D6
+#define MPU6050_XA_ST      MPU6050_D7
+
+// Combined definitions for the ACCEL_HPF values
+#define MPU6050_ACCEL_HPF_0 (0)
+#define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0))
+#define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1))
+#define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
+#define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2))
+#define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
+
+// Alternative names for the combined definitions
+// The name uses the Cut-off frequency.
+#define MPU6050_ACCEL_HPF_RESET  MPU6050_ACCEL_HPF_0
+#define MPU6050_ACCEL_HPF_5HZ    MPU6050_ACCEL_HPF_1
+#define MPU6050_ACCEL_HPF_2_5HZ  MPU6050_ACCEL_HPF_2
+#define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3
+#define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4
+#define MPU6050_ACCEL_HPF_HOLD   MPU6050_ACCEL_HPF_7
+
+// Combined definitions for the AFS_SEL values
+#define MPU6050_AFS_SEL_0 (0)
+#define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0))
+#define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1))
+#define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0))
+
+// Alternative names for the combined definitions
+// The name uses the full scale range for the accelerometer.
+#define MPU6050_AFS_SEL_2G  MPU6050_AFS_SEL_0
+#define MPU6050_AFS_SEL_4G  MPU6050_AFS_SEL_1
+#define MPU6050_AFS_SEL_8G  MPU6050_AFS_SEL_2
+#define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3
+
+// FIFO_EN Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_SLV0_FIFO_EN  MPU6050_D0
+#define MPU6050_SLV1_FIFO_EN  MPU6050_D1
+#define MPU6050_SLV2_FIFO_EN  MPU6050_D2
+#define MPU6050_ACCEL_FIFO_EN MPU6050_D3
+#define MPU6050_ZG_FIFO_EN    MPU6050_D4
+#define MPU6050_YG_FIFO_EN    MPU6050_D5
+#define MPU6050_XG_FIFO_EN    MPU6050_D6
+#define MPU6050_TEMP_FIFO_EN  MPU6050_D7
+
+// I2C_MST_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_MST_CLK0  MPU6050_D0
+#define MPU6050_I2C_MST_CLK1  MPU6050_D1
+#define MPU6050_I2C_MST_CLK2  MPU6050_D2
+#define MPU6050_I2C_MST_CLK3  MPU6050_D3
+#define MPU6050_I2C_MST_P_NSR MPU6050_D4
+#define MPU6050_SLV_3_FIFO_EN MPU6050_D5
+#define MPU6050_WAIT_FOR_ES   MPU6050_D6
+#define MPU6050_MULT_MST_EN   MPU6050_D7
+
+// Combined definitions for the I2C_MST_CLK
+#define MPU6050_I2C_MST_CLK_0 (0)
+#define MPU6050_I2C_MST_CLK_1  (bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_2  (bit(MPU6050_I2C_MST_CLK1))
+#define MPU6050_I2C_MST_CLK_3  (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_4  (bit(MPU6050_I2C_MST_CLK2))
+#define MPU6050_I2C_MST_CLK_5  (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_6  (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1))
+#define MPU6050_I2C_MST_CLK_7  (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_8  (bit(MPU6050_I2C_MST_CLK3))
+#define MPU6050_I2C_MST_CLK_9  (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1))
+#define MPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2))
+#define MPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0))
+#define MPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1))
+#define MPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
+
+// Alternative names for the combined definitions
+// The names uses I2C Master Clock Speed in kHz.
+#define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0
+#define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1
+#define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2
+#define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3
+#define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4
+#define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5
+#define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6
+#define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7
+#define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8
+#define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9
+#define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10
+#define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11
+#define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12
+#define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13
+#define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14
+#define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15
+
+// I2C_SLV0_ADDR Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV0_RW MPU6050_D7
+
+// I2C_SLV0_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV0_LEN0    MPU6050_D0
+#define MPU6050_I2C_SLV0_LEN1    MPU6050_D1
+#define MPU6050_I2C_SLV0_LEN2    MPU6050_D2
+#define MPU6050_I2C_SLV0_LEN3    MPU6050_D3
+#define MPU6050_I2C_SLV0_GRP     MPU6050_D4
+#define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5
+#define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6
+#define MPU6050_I2C_SLV0_EN      MPU6050_D7
+
+// A mask for the length
+#define MPU6050_I2C_SLV0_LEN_MASK 0x0F
+
+// I2C_SLV1_ADDR Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV1_RW MPU6050_D7
+
+// I2C_SLV1_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV1_LEN0    MPU6050_D0
+#define MPU6050_I2C_SLV1_LEN1    MPU6050_D1
+#define MPU6050_I2C_SLV1_LEN2    MPU6050_D2
+#define MPU6050_I2C_SLV1_LEN3    MPU6050_D3
+#define MPU6050_I2C_SLV1_GRP     MPU6050_D4
+#define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5
+#define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6
+#define MPU6050_I2C_SLV1_EN      MPU6050_D7
+
+// A mask for the length
+#define MPU6050_I2C_SLV1_LEN_MASK 0x0F
+
+// I2C_SLV2_ADDR Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV2_RW MPU6050_D7
+
+// I2C_SLV2_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV2_LEN0    MPU6050_D0
+#define MPU6050_I2C_SLV2_LEN1    MPU6050_D1
+#define MPU6050_I2C_SLV2_LEN2    MPU6050_D2
+#define MPU6050_I2C_SLV2_LEN3    MPU6050_D3
+#define MPU6050_I2C_SLV2_GRP     MPU6050_D4
+#define MPU6050_I2C_SLV2_REG_DIS MPU6050_D5
+#define MPU6050_I2C_SLV2_BYTE_SW MPU6050_D6
+#define MPU6050_I2C_SLV2_EN      MPU6050_D7
+
+// A mask for the length
+#define MPU6050_I2C_SLV2_LEN_MASK 0x0F
+
+// I2C_SLV3_ADDR Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV3_RW MPU6050_D7
+
+// I2C_SLV3_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV3_LEN0    MPU6050_D0
+#define MPU6050_I2C_SLV3_LEN1    MPU6050_D1
+#define MPU6050_I2C_SLV3_LEN2    MPU6050_D2
+#define MPU6050_I2C_SLV3_LEN3    MPU6050_D3
+#define MPU6050_I2C_SLV3_GRP     MPU6050_D4
+#define MPU6050_I2C_SLV3_REG_DIS MPU6050_D5
+#define MPU6050_I2C_SLV3_BYTE_SW MPU6050_D6
+#define MPU6050_I2C_SLV3_EN      MPU6050_D7
+
+// A mask for the length
+#define MPU6050_I2C_SLV3_LEN_MASK 0x0F
+
+// I2C_SLV4_ADDR Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV4_RW MPU6050_D7
+
+// I2C_SLV4_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_MST_DLY0     MPU6050_D0
+#define MPU6050_I2C_MST_DLY1     MPU6050_D1
+#define MPU6050_I2C_MST_DLY2     MPU6050_D2
+#define MPU6050_I2C_MST_DLY3     MPU6050_D3
+#define MPU6050_I2C_MST_DLY4     MPU6050_D4
+#define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5
+#define MPU6050_I2C_SLV4_INT_EN  MPU6050_D6
+#define MPU6050_I2C_SLV4_EN      MPU6050_D7
+
+// A mask for the delay
+#define MPU6050_I2C_MST_DLY_MASK 0x1F
+
+// I2C_MST_STATUS Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV0_NACK MPU6050_D0
+#define MPU6050_I2C_SLV1_NACK MPU6050_D1
+#define MPU6050_I2C_SLV2_NACK MPU6050_D2
+#define MPU6050_I2C_SLV3_NACK MPU6050_D3
+#define MPU6050_I2C_SLV4_NACK MPU6050_D4
+#define MPU6050_I2C_LOST_ARB  MPU6050_D5
+#define MPU6050_I2C_SLV4_DONE MPU6050_D6
+#define MPU6050_PASS_THROUGH  MPU6050_D7
+
+// I2C_PIN_CFG Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_CLKOUT_EN       MPU6050_D0
+#define MPU6050_I2C_BYPASS_EN   MPU6050_D1
+#define MPU6050_FSYNC_INT_EN    MPU6050_D2
+#define MPU6050_FSYNC_INT_LEVEL MPU6050_D3
+#define MPU6050_INT_RD_CLEAR    MPU6050_D4
+#define MPU6050_LATCH_INT_EN    MPU6050_D5
+#define MPU6050_INT_OPEN        MPU6050_D6
+#define MPU6050_INT_LEVEL       MPU6050_D7
+
+// INT_ENABLE Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_DATA_RDY_EN    MPU6050_D0
+#define MPU6050_I2C_MST_INT_EN MPU6050_D3
+#define MPU6050_FIFO_OFLOW_EN  MPU6050_D4
+#define MPU6050_ZMOT_EN        MPU6050_D5
+#define MPU6050_MOT_EN         MPU6050_D6
+#define MPU6050_FF_EN          MPU6050_D7
+
+// INT_STATUS Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_DATA_RDY_INT   MPU6050_D0
+#define MPU6050_I2C_MST_INT    MPU6050_D3
+#define MPU6050_FIFO_OFLOW_INT MPU6050_D4
+#define MPU6050_ZMOT_INT       MPU6050_D5
+#define MPU6050_MOT_INT        MPU6050_D6
+#define MPU6050_FF_INT         MPU6050_D7
+
+// MOT_DETECT_STATUS Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_MOT_ZRMOT MPU6050_D0
+#define MPU6050_MOT_ZPOS  MPU6050_D2
+#define MPU6050_MOT_ZNEG  MPU6050_D3
+#define MPU6050_MOT_YPOS  MPU6050_D4
+#define MPU6050_MOT_YNEG  MPU6050_D5
+#define MPU6050_MOT_XPOS  MPU6050_D6
+#define MPU6050_MOT_XNEG  MPU6050_D7
+
+// IC2_MST_DELAY_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0
+#define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1
+#define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2
+#define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3
+#define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4
+#define MPU6050_DELAY_ES_SHADOW MPU6050_D7
+
+// SIGNAL_PATH_RESET Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_TEMP_RESET  MPU6050_D0
+#define MPU6050_ACCEL_RESET MPU6050_D1
+#define MPU6050_GYRO_RESET  MPU6050_D2
+
+// MOT_DETECT_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_MOT_COUNT0      MPU6050_D0
+#define MPU6050_MOT_COUNT1      MPU6050_D1
+#define MPU6050_FF_COUNT0       MPU6050_D2
+#define MPU6050_FF_COUNT1       MPU6050_D3
+#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4
+#define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5
+
+// Combined definitions for the MOT_COUNT
+#define MPU6050_MOT_COUNT_0 (0)
+#define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0))
+#define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1))
+#define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0))
+
+// Alternative names for the combined definitions
+#define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0
+
+// Combined definitions for the FF_COUNT
+#define MPU6050_FF_COUNT_0 (0)
+#define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0))
+#define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1))
+#define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0))
+
+// Alternative names for the combined definitions
+#define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0
+
+// Combined definitions for the ACCEL_ON_DELAY
+#define MPU6050_ACCEL_ON_DELAY_0 (0)
+#define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0))
+#define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1))
+#define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0))
+
+// Alternative names for the ACCEL_ON_DELAY
+#define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0
+#define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1
+#define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2
+#define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3
+
+// USER_CTRL Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_SIG_COND_RESET MPU6050_D0
+#define MPU6050_I2C_MST_RESET  MPU6050_D1
+#define MPU6050_FIFO_RESET     MPU6050_D2
+#define MPU6050_I2C_IF_DIS     MPU6050_D4   // must be 0 for MPU-6050
+#define MPU6050_I2C_MST_EN     MPU6050_D5
+#define MPU6050_FIFO_EN        MPU6050_D6
+
+// PWR_MGMT_1 Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_CLKSEL0      MPU6050_D0
+#define MPU6050_CLKSEL1      MPU6050_D1
+#define MPU6050_CLKSEL2      MPU6050_D2
+#define MPU6050_TEMP_DIS     MPU6050_D3    // 1: disable temperature sensor
+#define MPU6050_CYCLE        MPU6050_D5    // 1: sample and sleep
+#define MPU6050_SLEEP        MPU6050_D6    // 1: sleep mode
+#define MPU6050_DEVICE_RESET MPU6050_D7    // 1: reset to default values
+
+// Combined definitions for the CLKSEL
+#define MPU6050_CLKSEL_0 (0)
+#define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0))
+#define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1))
+#define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
+#define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2))
+#define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0))
+#define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1))
+#define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
+
+// Alternative names for the combined definitions
+#define MPU6050_CLKSEL_INTERNAL    MPU6050_CLKSEL_0
+#define MPU6050_CLKSEL_X           MPU6050_CLKSEL_1
+#define MPU6050_CLKSEL_Y           MPU6050_CLKSEL_2
+#define MPU6050_CLKSEL_Z           MPU6050_CLKSEL_3
+#define MPU6050_CLKSEL_EXT_32KHZ   MPU6050_CLKSEL_4
+#define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5
+#define MPU6050_CLKSEL_RESERVED    MPU6050_CLKSEL_6
+#define MPU6050_CLKSEL_STOP        MPU6050_CLKSEL_7
+
+// PWR_MGMT_2 Register
+// These are the names for the bits.
+// Use these only with the bit() macro.
+#define MPU6050_STBY_ZG       MPU6050_D0
+#define MPU6050_STBY_YG       MPU6050_D1
+#define MPU6050_STBY_XG       MPU6050_D2
+#define MPU6050_STBY_ZA       MPU6050_D3
+#define MPU6050_STBY_YA       MPU6050_D4
+#define MPU6050_STBY_XA       MPU6050_D5
+#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6
+#define MPU6050_LP_WAKE_CTRL1 MPU6050_D7
+
+// Combined definitions for the LP_WAKE_CTRL
+#define MPU6050_LP_WAKE_CTRL_0 (0)
+#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0))
+#define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1))
+#define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0))
+
+// Alternative names for the combined definitions
+// The names uses the Wake-up Frequency.
+#define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0
+#define MPU6050_LP_WAKE_2_5HZ  MPU6050_LP_WAKE_CTRL_1
+#define MPU6050_LP_WAKE_5HZ    MPU6050_LP_WAKE_CTRL_2
+#define MPU6050_LP_WAKE_10HZ   MPU6050_LP_WAKE_CTRL_3
+
+
+// Default I2C address for the MPU-6050 is 0x68.
+// But only if the AD0 pin is low.
+// Some sensor boards have AD0 high, and the
+// I2C address thus becomes 0x69.
+#define MPU6050_I2C_ADDRESS 0x69
+
+ int flag=6;
+// Declaring an union for the registers and the axis values.
+// The byte order does not match the byte order of
+// the compiler and AVR chip.
+// The AVR chip (on the Arduino board) has the Low Byte
+// at the lower address.
+// But the MPU-6050 has a different order: High Byte at
+// lower address, so that has to be corrected.
+// The register part "reg" is only used internally,
+// and are swapped in code.
+
+typedef union accel_t_gyro_union
+{
+  struct
+  {
+    uint8_t x_accel_h;
+    uint8_t x_accel_l;
+    uint8_t y_accel_h;
+    uint8_t y_accel_l;
+    uint8_t z_accel_h;
+    uint8_t z_accel_l;
+    uint8_t x_gyro_h;
+    uint8_t x_gyro_l;
+    uint8_t y_gyro_h;
+    uint8_t y_gyro_l;
+    uint8_t z_gyro_h;
+    uint8_t z_gyro_l;
+  } reg;
+  struct
+  {
+    int16_t x_accel;
+    int16_t y_accel;
+    int16_t z_accel;
+    int16_t x_gyro;
+    int16_t y_gyro;
+    int16_t z_gyro;
+  } value;
+};
+
+int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.  Returns the error value
+
+  accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr;
+
+  int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro));
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l);
+  SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l);
+  SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l);
+  SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l);
+  SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l);
+  SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l);
+
+  return error;
+}
+
+unsigned long last_read_time[5];
+float         last_x_angle[5];  // These are the filtered angles
+float         last_y_angle[5];
+float         last_z_angle[5];
+float         last_gyro_x_angle[5];  // Store the gyro angles to compare drift
+float         last_gyro_y_angle[5];
+float         last_gyro_z_angle[5];
+
+
+void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro, int a) {
+  last_read_time[a] = time;
+  last_x_angle[a] = x;
+  last_y_angle[a] = y;
+  last_z_angle[a] = z;
+  last_gyro_x_angle[a] = x_gyro;
+  last_gyro_y_angle[a] = y_gyro;
+  last_gyro_z_angle[a] = z_gyro;
+}
+
+inline unsigned long get_last_time(int a) {return last_read_time[a];}
+inline float get_last_x_angle(int a) {return last_x_angle[a];}
+inline float get_last_y_angle(int a) {return last_y_angle[a];}
+inline float get_last_z_angle(int a) {return last_z_angle[a];}
+inline float get_last_gyro_x_angle(int a) {return last_gyro_x_angle[a];}
+inline float get_last_gyro_y_angle(int a) {return last_gyro_y_angle[a];}
+inline float get_last_gyro_z_angle(int a) {return last_gyro_z_angle[a];}
+
+float    base_x_accel[5];
+float    base_y_accel[5];
+float    base_z_accel[5];
+
+float    base_x_gyro[5];
+float    base_y_gyro[5];
+float    base_z_gyro[5];
+
+
+const int MPU_addr=0x69;  // I2C address of the MPU-6050
+//int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
+
+void setup(){
+ int error;
+  uint8_t c;
+  Wire.begin();
+
+  Serial.begin(115200);
+  Serial.println(F("InvenSense MPU-6050"));
+  Serial.println(F("June 2012"));
+
+  while (!Serial);             // Leonardo: wait for serial monitor
+  int i;
+  pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+    for(i=6;i<=9;i++)
+    {
+      for(int j=6;j<=12;j++)
+      {
+        if(i!=j)
+        {
+          pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+        }
+      }
+      pinMode(i,OUTPUT);
+      digitalWrite(i,HIGH);
+  error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
+  Serial.print(F("WHO_AM_I : "));
+  Serial.print(c,HEX);
+  Serial.print(F(", error = "));
+  Serial.println(error,DEC);
+
+  // According to the datasheet, the 'sleep' bit
+  // should read a '1'.
+  // That bit has to be cleared, since the sensor
+  // is in sleep mode at power-up.
+  error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);
+  Serial.print(F("PWR_MGMT_1 : "));
+  Serial.print(c,HEX);
+  Serial.print(F(", error = "));
+  Serial.println(error,DEC);
+
+
+  // Clear the 'sleep' bit to start the sensor.
+  MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
+  Serial.println("\nI2C Scanner");
+    }
+    pinMode(9,OUTPUT);
+        digitalWrite(9,LOW);
+   pinMode(12,OUTPUT);
+        digitalWrite(12,HIGH);
+        error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
+  Serial.print(F("WHO_AM_I : "));
+  Serial.print(c,HEX);
+  Serial.print(F(", error = "));
+  Serial.println(error,DEC);
+
+  // According to the datasheet, the 'sleep' bit
+  // should read a '1'.
+  // That bit has to be cleared, since the sensor
+  // is in sleep mode at power-up.
+  error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);
+  Serial.print(F("PWR_MGMT_1 : "));
+  Serial.print(c,HEX);
+  Serial.print(F(", error = "));
+  Serial.println(error,DEC);
+
+
+  // Clear the 'sleep' bit to start the sensor.
+  MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
+      mySerial.begin(115200);
+      calibrate_sensor1();
+      set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 0);
+      calibrate_sensor2();
+      set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 1);
+      calibrate_sensor3();
+      set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 2);
+      calibrate_sensor4();
+      set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 3);
+      calibrate_sensor5();
+      set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 4);
+}
+
+
+
+
+void loop(){
+
+byte error, address;
+address=105;
+  //int nDevices;
+
+  //Serial.println(F("Scanning I2C bus (7-bit addresses) ..."));
+  //nDevices = 0;
+      if(flag==6)
+      {
+        mpunumber1();
+        //delay(1000);
+      }
+      if(flag==7)
+      {
+        mpunumber2();
+        //delay(1000);
+      }
+       if(flag==8)
+       {
+        mpunumber3();
+        //delay(1000);
+      }
+       if(flag==9)
+        {
+        mpunumber4();
+        //delay(1000);
+      }
+
+       if(flag==10)
+       {
+        mpunumber5();
+        flag=6;
+        //delay(1000);
+      }
+       else
+        ++flag;
+        mySerial.flush();
+      // The i2c_scanner uses the return value of
+      // the Write.endTransmisstion to see if
+      // a device did acknowledge to the address.
+      /*Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      if (error == 0)
+        {
+          Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+
+  Serial.println(F(""));
+  Serial.println(F("MPU-6050"));
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+  Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
+
+
+  // Print the raw acceleration values
+
+  Serial.print(F("accel x,y,z: "));
+  mySerial.println("The Accl X value is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));
+
+
+  // Print the raw gyro values.
+
+  Serial.print(F("gyro x,y,z : "));
+  mySerial.println("The Gyro X value is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");
+
+          nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+  if (nDevices == 0)
+    Serial.println("No I2C devices found\n");
+  else
+    Serial.println("done\n");
+
+  if(flag==10)
+    flag=6;
+    else
+      flag++;
+  //delay(5000);           // wait 5 seconds for next scan*/
+}
+
+
+// --------------------------------------------------------
+// MPU6050_read
+//
+// This is a common function to read multiple bytes
+// from an I2C device.
+//
+// It uses the boolean parameter for Wire.endTransMission()
+// to be able to hold or release the I2C-bus.
+// This is implemented in Arduino 1.0.1.
+//
+// Only this function is used to read.
+// There is no function for a single byte.
+//
+int MPU6050_read(int start, uint8_t *buffer, int size)
+{
+  int i, n, error;
+
+  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
+  n = Wire.write(start);
+  if (n != 1)
+    return (-10);
+
+  n = Wire.endTransmission(false);    // hold the I2C-bus
+  if (n != 0)
+    return (n);
+
+  // Third parameter is true: relase I2C-bus after data is read.
+  Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
+  i = 0;
+  while(Wire.available() && i<size)
+  {
+    buffer[i++]=Wire.read();
+  }
+  if ( i != size)
+    return (-11);
+
+  return (0);  // return : no error
+}
+
+
+// --------------------------------------------------------
+// MPU6050_write
+//
+// This is a common function to write multiple bytes to an I2C device.
+//
+// If only a single register is written,
+// use the function MPU_6050_write_reg().
+//
+// Parameters:
+//   start : Start address, use a define for the register
+//   pData : A pointer to the data to write.
+//   size  : The number of bytes to write.
+//
+// If only a single register is written, a pointer
+// to the data has to be used, and the size is
+// a single byte:
+//   int data = 0;        // the data to write
+//   MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
+//
+int MPU6050_write(int start, const uint8_t *pData, int size)
+{
+  int n, error;
+
+  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
+  n = Wire.write(start);        // write the start address
+  if (n != 1)
+    return (-20);
+
+  n = Wire.write(pData, size);  // write data bytes
+  if (n != size)
+    return (-21);
+
+  error = Wire.endTransmission(true); // release the I2C-bus
+  if (error != 0)
+    return (error);
+
+  return (0);         // return : no error
+}
+
+// --------------------------------------------------------
+// MPU6050_write_reg
+//
+// An extra function to write a single register.
+// It is just a wrapper around the MPU_6050_write()
+// function, and it is only a convenient function
+// to make it easier to write a single register.
+//
+int MPU6050_write_reg(int reg, uint8_t data)
+{
+  int error;
+
+  error = MPU6050_write(reg, &data, 1);
+
+  return (error);
+}
+
+
+void mpunumber1()
+{
+  mySerial.println("It is entering the Thumb Function");
+  for(int j=7;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(6,OUTPUT);
+        digitalWrite(6,HIGH);
+    byte error, address;
+    address=105;
+    Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      mySerial.println("The error is :");
+      mySerial.println(error);
+      if (error == 0)
+        {
+          /*Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));*/
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+  error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
+  unsigned long t_now = millis();
+  /*Serial.println(F(""));
+  Serial.println(F("MPU-6050"));*/
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  //error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+ /* Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);*/
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  /*uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);*/
+
+
+  // Print the raw acceleration values
+
+ // Serial.print(F("accel x,y,z: "));
+  /*mySerial.println("The Accl X value from Thumb is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value from Thumb is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value from Thumb is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");*/
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));*/
+
+
+  // Print the raw gyro values.
+
+  //Serial.print(F("gyro x,y,z : "));
+  /*mySerial.println("The Gyro X value from Thumb is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");*/
+        //mySerial.println("Thumb angle values");
+    float FS_SEL = 131;
+
+  float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro[0])/FS_SEL;
+  float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro[0])/FS_SEL;
+  float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro[0])/FS_SEL;
+
+
+  // Get raw acceleration values
+  //float G_CONVERT = 16384;
+  float accel_x = accel_t_gyro.value.x_accel;
+  float accel_y = accel_t_gyro.value.y_accel;
+  float accel_z = accel_t_gyro.value.z_accel;
+
+  // Get angle values from accelerometer
+  float RADIANS_TO_DEGREES = 180/3.14159;
+//  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
+  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+
+  float accel_angle_z = 0;
+
+  // Compute the (filtered) gyro angles
+  float dt =(t_now - get_last_time(0))/1000.0;
+  float gyro_angle_x = gyro_x*dt + get_last_x_angle(0);
+  float gyro_angle_y = gyro_y*dt + get_last_y_angle(0);
+  float gyro_angle_z = gyro_z*dt + get_last_z_angle(0);
+
+  // Compute the drifting gyro angles
+  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(0);
+  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(0);
+  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(0);
+
+  // Apply the complementary filter to figure out the change in angle - choice of alpha is
+  // estimated now.  Alpha depends on the sampling rate...
+  float alpha = 0.96;
+  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
+  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
+  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
+
+  // Update the saved data with the latest values
+  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, 0);
+
+  // Send the data to the serial port
+ /* mySerial.print(F("DEL:"));              //Delta T
+  mySerial.print(dt, DEC);
+  mySerial.print(F("#ACC:"));              //Accelerometer angle
+  mySerial.print(accel_angle_x, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_y, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_z, 2);
+  mySerial.print(F("#GYR:"));
+  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_y, 2);
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_z, 2);*/
+  mySerial.println(F("@ "));             //Filtered angle
+  mySerial.print(angle_x, DEC);
+  mySerial.println(F(""));
+  mySerial.print(angle_y, DEC);
+  mySerial.println(F(""));
+  mySerial.print(angle_z, DEC);
+  mySerial.println(F(""));
+
+  // Delay so we don't swamp the serial port
+
+         // nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+
+}
+
+
+void mpunumber2()
+{
+  for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(7,OUTPUT);
+        digitalWrite(7,HIGH);
+    byte error, address;
+    address=105;
+    Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      if (error == 0)
+        {
+          /*Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));*/
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+  error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
+  unsigned long t_now = millis();
+  /*Serial.println(F(""));
+  Serial.println(F("MPU-6050"));*/
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  //error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+ /* Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);*/
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  /*uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);*/
+
+
+  // Print the raw acceleration values
+
+ // Serial.print(F("accel x,y,z: "));
+  /*mySerial.println("The Accl X value from Thumb is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value from Thumb is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value from Thumb is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");*/
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));*/
+
+
+  // Print the raw gyro values.
+
+  //Serial.print(F("gyro x,y,z : "));
+  /*mySerial.println("The Gyro X value from Thumb is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");*/
+        //mySerial.println("Thumb angle values");
+    float FS_SEL = 131;
+
+  float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro[1])/FS_SEL;
+  float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro[1])/FS_SEL;
+  float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro[1])/FS_SEL;
+
+
+  // Get raw acceleration values
+  //float G_CONVERT = 16384;
+  float accel_x = accel_t_gyro.value.x_accel;
+  float accel_y = accel_t_gyro.value.y_accel;
+  float accel_z = accel_t_gyro.value.z_accel;
+
+  // Get angle values from accelerometer
+  float RADIANS_TO_DEGREES = 180/3.14159;
+//  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
+  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+
+  float accel_angle_z = 0;
+
+  // Compute the (filtered) gyro angles
+  float dt =(t_now - get_last_time(1))/1000.0;
+  float gyro_angle_x = gyro_x*dt + get_last_x_angle(1);
+  float gyro_angle_y = gyro_y*dt + get_last_y_angle(1);
+  float gyro_angle_z = gyro_z*dt + get_last_z_angle(1);
+
+  // Compute the drifting gyro angles
+  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(1);
+  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(1);
+  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(1);
+
+  // Apply the complementary filter to figure out the change in angle - choice of alpha is
+  // estimated now.  Alpha depends on the sampling rate...
+  float alpha = 0.96;
+  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
+  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
+  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
+
+  // Update the saved data with the latest values
+  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, 1);
+
+  // Send the data to the serial port
+ /* mySerial.print(F("DEL:"));              //Delta T
+  mySerial.print(dt, DEC);
+  mySerial.print(F("#ACC:"));              //Accelerometer angle
+  mySerial.print(accel_angle_x, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_y, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_z, 2);
+  mySerial.print(F("#GYR:"));
+  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_y, 2);
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_z, 2);*/
+  mySerial.println(F("#"));             //Filtered angle
+  mySerial.print(angle_x, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_y, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_z, DEC);
+  mySerial.println(F(""));
+
+  // Delay so we don't swamp the serial port
+
+         // nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+
+}
+
+void mpunumber3()
+{
+  for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(8,OUTPUT);
+        digitalWrite(8,HIGH);
+  byte error, address;
+    address=105;
+    Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      if (error == 0)
+        {
+          /*Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));*/
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+  error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
+  unsigned long t_now = millis();
+  /*Serial.println(F(""));
+  Serial.println(F("MPU-6050"));*/
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  //error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+ /* Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);*/
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  /*uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);*/
+
+
+  // Print the raw acceleration values
+
+ // Serial.print(F("accel x,y,z: "));
+  /*mySerial.println("The Accl X value from Thumb is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value from Thumb is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value from Thumb is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");*/
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));*/
+
+
+  // Print the raw gyro values.
+
+  //Serial.print(F("gyro x,y,z : "));
+  /*mySerial.println("The Gyro X value from Thumb is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");*/
+        //mySerial.println(" angle values");
+    float FS_SEL = 131;
+
+  float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro[2])/FS_SEL;
+  float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro[2])/FS_SEL;
+  float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro[2])/FS_SEL;
+
+
+  // Get raw acceleration values
+  //float G_CONVERT = 16384;
+  float accel_x = accel_t_gyro.value.x_accel;
+  float accel_y = accel_t_gyro.value.y_accel;
+  float accel_z = accel_t_gyro.value.z_accel;
+
+  // Get angle values from accelerometer
+  float RADIANS_TO_DEGREES = 180/3.14159;
+//  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
+  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+
+  float accel_angle_z = 0;
+
+  // Compute the (filtered) gyro angles
+  float dt =(t_now - get_last_time(2))/1000.0;
+  float gyro_angle_x = gyro_x*dt + get_last_x_angle(2);
+  float gyro_angle_y = gyro_y*dt + get_last_y_angle(2);
+  float gyro_angle_z = gyro_z*dt + get_last_z_angle(2);
+
+  // Compute the drifting gyro angles
+  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(2);
+  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(2);
+  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(2);
+
+  // Apply the complementary filter to figure out the change in angle - choice of alpha is
+  // estimated now.  Alpha depends on the sampling rate...
+  float alpha = 0.96;
+  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
+  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
+  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
+
+  // Update the saved data with the latest values
+  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, 2);
+
+  // Send the data to the serial port
+ /* mySerial.print(F("DEL:"));              //Delta T
+  mySerial.print(dt, DEC);
+  mySerial.print(F("#ACC:"));              //Accelerometer angle
+  mySerial.print(accel_angle_x, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_y, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_z, 2);
+  mySerial.print(F("#GYR:"));
+  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_y, 2);
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_z, 2);*/
+  mySerial.println(F("$"));             //Filtered angle
+  mySerial.print(angle_x, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_y, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_z, DEC);
+  mySerial.println(F(""));
+
+  // Delay so we don't swamp the serial port
+
+         // nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+
+}
+
+void mpunumber4()
+{
+  for(int j=6;j<=8;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(9,OUTPUT);
+        digitalWrite(9,HIGH);
+  byte error, address;
+    address=105;
+    Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      if (error == 0)
+        {
+          /*Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));*/
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+  error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
+  unsigned long t_now = millis();
+  /*Serial.println(F(""));
+  Serial.println(F("MPU-6050"));*/
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  //error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+ /* Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);*/
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  /*uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);*/
+
+
+  // Print the raw acceleration values
+
+ // Serial.print(F("accel x,y,z: "));
+  /*mySerial.println("The Accl X value from Thumb is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value from Thumb is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value from Thumb is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");*/
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));*/
+
+
+  // Print the raw gyro values.
+
+  //Serial.print(F("gyro x,y,z : "));
+  /*mySerial.println("The Gyro X value from Thumb is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");*/
+        //mySerial.println("Thumb angle values");
+    float FS_SEL = 131;
+
+  float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro[3])/FS_SEL;
+  float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro[3])/FS_SEL;
+  float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro[3])/FS_SEL;
+
+
+  // Get raw acceleration values
+  //float G_CONVERT = 16384;
+  float accel_x = accel_t_gyro.value.x_accel;
+  float accel_y = accel_t_gyro.value.y_accel;
+  float accel_z = accel_t_gyro.value.z_accel;
+
+  // Get angle values from accelerometer
+  float RADIANS_TO_DEGREES = 180/3.14159;
+//  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
+  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+
+  float accel_angle_z = 0;
+
+  // Compute the (filtered) gyro angles
+  float dt =(t_now - get_last_time(3))/1000.0;
+  float gyro_angle_x = gyro_x*dt + get_last_x_angle(3);
+  float gyro_angle_y = gyro_y*dt + get_last_y_angle(3);
+  float gyro_angle_z = gyro_z*dt + get_last_z_angle(3);
+
+  // Compute the drifting gyro angles
+  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(3);
+  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(3);
+  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(3);
+
+  // Apply the complementary filter to figure out the change in angle - choice of alpha is
+  // estimated now.  Alpha depends on the sampling rate...
+  float alpha = 0.96;
+  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
+  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
+  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
+
+
+  // Update the saved data with the latest values
+  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, 3);
+
+  // Send the data to the serial port
+ /* mySerial.print(F("DEL:"));              //Delta T
+  mySerial.print(dt, DEC);
+  mySerial.print(F("#ACC:"));              //Accelerometer angle
+  mySerial.print(accel_angle_x, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_y, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_z, 2);
+  mySerial.print(F("#GYR:"));
+  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_y, 2);
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_z, 2);*/
+  mySerial.println(F("% "));             //Filtered angle
+  mySerial.print(angle_x, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_y, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_z, DEC);
+  mySerial.println(F(""));
+
+  // Delay so we don't swamp the serial port
+
+         // nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+
+}
+
+void mpunumber5()
+{
+  for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      //pinMode(12,OUTPUT);
+        //digitalWrite(12,LOW);
+  pinMode(12,OUTPUT);
+        digitalWrite(12,HIGH);
+  byte error, address;
+    address=105;
+    Wire.beginTransmission(address);
+      error = Wire.endTransmission();
+
+      if (error == 0)
+        {
+          /*Serial.print("MPU number : ");
+          Serial.println(flag-5);
+          Serial.print(F("I2C device found at address 0x"));
+          if (address<16)
+            Serial.print(F("0"));
+          Serial.print(address,HEX);
+          Serial.println(F("  !"));*/
+
+  int error;
+  double dT;
+  accel_t_gyro_union accel_t_gyro;
+
+  error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
+  unsigned long t_now = millis();
+  /*Serial.println(F(""));
+  Serial.println(F("MPU-6050"));*/
+
+  // Read the raw values.
+  // Read 14 bytes at once,
+  // containing acceleration, temperature and gyro.
+  // With the default settings of the MPU-6050,
+  // there is no filter enabled, and the values
+  // are not very stable.
+  //error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
+ /* Serial.print(F("Read accel, temp and gyro, error = "));
+  Serial.println(error,DEC);*/
+
+
+  // Swap all high and low bytes.
+  // After this, the registers values are swapped,
+  // so the structure name like x_accel_l does no
+  // longer contain the lower byte.
+  /*uint8_t swap;
+  #define SWAP(x,y) swap = x; x = y; y = swap
+
+  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
+  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
+  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
+  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
+  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
+  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);*/
+
+
+  // Print the raw acceleration values
+
+ // Serial.print(F("accel x,y,z: "));
+  /*mySerial.println("The Accl X value from Thumb is : ");
+  //Serial.print(accel_t_gyro.value.x_accel, DEC);
+  //Serial.print(F(", "));
+    mySerial.print(accel_t_gyro.value.x_accel);
+  mySerial.print("\n");
+    mySerial.println("The Accl Y value from Thumb is : ");
+//Serial.print(accel_t_gyro.value.y_accel, DEC);
+      mySerial.print(accel_t_gyro.value.y_accel);
+  mySerial.print("\n");
+
+ // Serial.print(F(", "));
+   mySerial.println("The Accl Z value from Thumb is : ");
+
+  //Serial.print(accel_t_gyro.value.z_accel, DEC);
+     mySerial.print(accel_t_gyro.value.z_accel);
+
+  //Serial.println(F(""));
+    mySerial.print("\n");*/
+
+
+
+  // The temperature sensor is -40 to +85 degrees Celsius.
+  // It is a signed integer.
+  // According to the datasheet:
+  //   340 per degrees Celsius, -512 at 35 degrees.
+  // At 0 degrees: -512 - (340 * 35) = -12412
+
+  /*Serial.print(F("temperature: "));
+  dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
+  Serial.print(dT, 3);
+  Serial.print(F(" degrees Celsius"));
+  Serial.println(F(""));*/
+
+
+  // Print the raw gyro values.
+
+  //Serial.print(F("gyro x,y,z : "));
+  /*mySerial.println("The Gyro X value from Thumb is : ");
+  mySerial.print(accel_t_gyro.value.x_gyro);
+      mySerial.print("\n");
+  //Serial.print(accel_t_gyro.value.x_gyro, DEC);
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.y_gyro, DEC);
+    mySerial.println("The Gyro Y value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.y_gyro);
+        mySerial.print("\n");
+
+  //Serial.print(F(", "));
+  //Serial.print(accel_t_gyro.value.z_gyro, DEC);
+    mySerial.println("The Gyro Z value from Thumb is : ");
+    mySerial.print(accel_t_gyro.value.z_gyro);
+        mySerial.print("\n");*/
+        //mySerial.println("Thumb angle values");
+    float FS_SEL = 131;
+
+  float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro[4])/FS_SEL;
+  float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro[4])/FS_SEL;
+  float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro[4])/FS_SEL;
+
+
+  // Get raw acceleration values
+  //float G_CONVERT = 16384;
+  float accel_x = accel_t_gyro.value.x_accel;
+  float accel_y = accel_t_gyro.value.y_accel;
+  float accel_z = accel_t_gyro.value.z_accel;
+
+  // Get angle values from accelerometer
+  float RADIANS_TO_DEGREES = 180/3.14159;
+//  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
+  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
+
+  float accel_angle_z = 0;
+
+  // Compute the (filtered) gyro angles
+  float dt =(t_now - get_last_time(4))/1000.0;
+  float gyro_angle_x = gyro_x*dt + get_last_x_angle(4);
+  float gyro_angle_y = gyro_y*dt + get_last_y_angle(4);
+  float gyro_angle_z = gyro_z*dt + get_last_z_angle(4);
+
+  // Compute the drifting gyro angles
+  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(4);
+  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(4);
+  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(4);
+
+  // Apply the complementary filter to figure out the change in angle - choice of alpha is
+  // estimated now.  Alpha depends on the sampling rate...
+  float alpha = 0.96;
+  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
+  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
+  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
+
+  // Update the saved data with the latest values
+  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, 4);
+
+  // Send the data to the serial port
+ /* mySerial.print(F("DEL:"));              //Delta T
+  mySerial.print(dt, DEC);
+  mySerial.print(F("#ACC:"));              //Accelerometer angle
+  mySerial.print(accel_angle_x, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_y, 2);
+  mySerial.print(F(","));
+  mySerial.print(accel_angle_z, 2);
+  mySerial.print(F("#GYR:"));
+  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_y, 2);
+  Serial.print(F(","));
+  Serial.print(unfiltered_gyro_angle_z, 2);*/
+  mySerial.println(F("^"));             //Filtered angle
+  mySerial.print(angle_x, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_y, DEC);
+  mySerial.println(F(","));
+  mySerial.print(angle_z, DEC);
+  mySerial.println(F(""));
+
+  // Delay so we don't swamp the serial port
+
+         // nDevices++;
+
+        }
+      else if (error==4)
+        {
+          Serial.print(F("Unknown error at address 0x"));
+          if (address<16)
+            Serial.print("0");
+          Serial.println(address,HEX);
+        }
+
+
+
+}
+
+
+void calibrate_sensor1()
+{
+  int                   num_readings = 10;
+  float                 x_accel = 0;
+  float                 y_accel = 0;
+  float                 z_accel = 0;
+  float                 x_gyro = 0;
+  float                 y_gyro = 0;
+  float                 z_gyro = 0;
+  accel_t_gyro_union    accel_t_gyro;
+
+for(int j=7;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(6,OUTPUT);
+        digitalWrite(6,HIGH);
+  //Serial.println("Starting Calibration");
+
+  // Discard the first set of values read from the IMU
+  read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+
+  // Read and average the raw values from the IMU
+  for (int i = 0; i < num_readings; i++) {
+    read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+    x_accel += accel_t_gyro.value.x_accel;
+    y_accel += accel_t_gyro.value.y_accel;
+    z_accel += accel_t_gyro.value.z_accel;
+    x_gyro += accel_t_gyro.value.x_gyro;
+    y_gyro += accel_t_gyro.value.y_gyro;
+    z_gyro += accel_t_gyro.value.z_gyro;
+    delay(100);
+  }
+  x_accel /= num_readings;
+  y_accel /= num_readings;
+  z_accel /= num_readings;
+  x_gyro /= num_readings;
+  y_gyro /= num_readings;
+  z_gyro /= num_readings;
+
+  // Store the raw calibration values globally
+  base_x_accel[0] = x_accel;
+  base_y_accel[0] = y_accel;
+  base_z_accel[0] = z_accel;
+  base_x_gyro[0] = x_gyro;
+  base_y_gyro[0] = y_gyro;
+  base_z_gyro[0] = z_gyro;
+
+
+
+}
+void calibrate_sensor2()
+{
+  int                   num_readings = 10;
+  float                 x_accel = 0;
+  float                 y_accel = 0;
+  float                 z_accel = 0;
+  float                 x_gyro = 0;
+  float                 y_gyro = 0;
+  float                 z_gyro = 0;
+  accel_t_gyro_union    accel_t_gyro;
+
+for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(7,OUTPUT);
+        digitalWrite(7,HIGH);
+  //Serial.println("Starting Calibration");
+
+  // Discard the first set of values read from the IMU
+  read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+
+  // Read and average the raw values from the IMU
+  for (int i = 0; i < num_readings; i++) {
+    read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+    x_accel += accel_t_gyro.value.x_accel;
+    y_accel += accel_t_gyro.value.y_accel;
+    z_accel += accel_t_gyro.value.z_accel;
+    x_gyro += accel_t_gyro.value.x_gyro;
+    y_gyro += accel_t_gyro.value.y_gyro;
+    z_gyro += accel_t_gyro.value.z_gyro;
+    delay(100);
+  }
+  x_accel /= num_readings;
+  y_accel /= num_readings;
+  z_accel /= num_readings;
+  x_gyro /= num_readings;
+  y_gyro /= num_readings;
+  z_gyro /= num_readings;
+
+  // Store the raw calibration values globally
+  base_x_accel[1] = x_accel;
+  base_y_accel[1] = y_accel;
+  base_z_accel[1] = z_accel;
+  base_x_gyro[1] = x_gyro;
+  base_y_gyro[1] = y_gyro;
+  base_z_gyro[1] = z_gyro;
+}
+void calibrate_sensor3()
+{
+    int                   num_readings = 10;
+  float                 x_accel = 0;
+  float                 y_accel = 0;
+  float                 z_accel = 0;
+  float                 x_gyro = 0;
+  float                 y_gyro = 0;
+  float                 z_gyro = 0;
+  accel_t_gyro_union    accel_t_gyro;
+
+for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(8,OUTPUT);
+        digitalWrite(8,HIGH);
+  //Serial.println("Starting Calibration");
+
+  // Discard the first set of values read from the IMU
+  read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+
+  // Read and average the raw values from the IMU
+  for (int i = 0; i < num_readings; i++) {
+    read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+    x_accel += accel_t_gyro.value.x_accel;
+    y_accel += accel_t_gyro.value.y_accel;
+    z_accel += accel_t_gyro.value.z_accel;
+    x_gyro += accel_t_gyro.value.x_gyro;
+    y_gyro += accel_t_gyro.value.y_gyro;
+    z_gyro += accel_t_gyro.value.z_gyro;
+    delay(100);
+  }
+  x_accel /= num_readings;
+  y_accel /= num_readings;
+  z_accel /= num_readings;
+  x_gyro /= num_readings;
+  y_gyro /= num_readings;
+  z_gyro /= num_readings;
+
+  // Store the raw calibration values globally
+  base_x_accel[2] = x_accel;
+  base_y_accel[2] = y_accel;
+  base_z_accel[2] = z_accel;
+  base_x_gyro[2] = x_gyro;
+  base_y_gyro[2] = y_gyro;
+  base_z_gyro[2] = z_gyro;
+}
+void calibrate_sensor4()
+{
+    int                   num_readings = 10;
+  float                 x_accel = 0;
+  float                 y_accel = 0;
+  float                 z_accel = 0;
+  float                 x_gyro = 0;
+  float                 y_gyro = 0;
+  float                 z_gyro = 0;
+  accel_t_gyro_union    accel_t_gyro;
+
+for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+      pinMode(12,OUTPUT);
+        digitalWrite(12,LOW);
+  pinMode(9,OUTPUT);
+        digitalWrite(9,HIGH);
+  //Serial.println("Starting Calibration");
+
+  // Discard the first set of values read from the IMU
+  read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+
+  // Read and average the raw values from the IMU
+  for (int i = 0; i < num_readings; i++) {
+    read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+    x_accel += accel_t_gyro.value.x_accel;
+    y_accel += accel_t_gyro.value.y_accel;
+    z_accel += accel_t_gyro.value.z_accel;
+    x_gyro += accel_t_gyro.value.x_gyro;
+    y_gyro += accel_t_gyro.value.y_gyro;
+    z_gyro += accel_t_gyro.value.z_gyro;
+    delay(100);
+  }
+  x_accel /= num_readings;
+  y_accel /= num_readings;
+  z_accel /= num_readings;
+  x_gyro /= num_readings;
+  y_gyro /= num_readings;
+  z_gyro /= num_readings;
+
+  // Store the raw calibration values globally
+  base_x_accel[3] = x_accel;
+  base_y_accel[3] = y_accel;
+  base_z_accel[3] = z_accel;
+  base_x_gyro[3] = x_gyro;
+  base_y_gyro[3] = y_gyro;
+  base_z_gyro[3] = z_gyro;
+}
+void calibrate_sensor5()
+{
+    int                   num_readings = 10;
+  float                 x_accel = 0;
+  float                 y_accel = 0;
+  float                 z_accel = 0;
+  float                 x_gyro = 0;
+  float                 y_gyro = 0;
+  float                 z_gyro = 0;
+  accel_t_gyro_union    accel_t_gyro;
+
+for(int j=6;j<=9;j++)
+      {
+
+        pinMode(j,OUTPUT);
+        digitalWrite(j,LOW);
+
+      }
+  pinMode(12,OUTPUT);
+        digitalWrite(12,HIGH);
+  //Serial.println("Starting Calibration");
+
+  // Discard the first set of values read from the IMU
+  read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+
+  // Read and average the raw values from the IMU
+  for (int i = 0; i < num_readings; i++) {
+    read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
+    x_accel += accel_t_gyro.value.x_accel;
+    y_accel += accel_t_gyro.value.y_accel;
+    z_accel += accel_t_gyro.value.z_accel;
+    x_gyro += accel_t_gyro.value.x_gyro;
+    y_gyro += accel_t_gyro.value.y_gyro;
+    z_gyro += accel_t_gyro.value.z_gyro;
+    delay(100);
+  }
+  x_accel /= num_readings;
+  y_accel /= num_readings;
+  z_accel /= num_readings;
+  x_gyro /= num_readings;
+  y_gyro /= num_readings;
+  z_gyro /= num_readings;
+
+  // Store the raw calibration values globally
+  base_x_accel[4] = x_accel;
+  base_y_accel[4] = y_accel;
+  base_z_accel[4] = z_accel;
+  base_x_gyro[4] = x_gyro;
+  base_y_gyro[4] = y_gyro;
+  base_z_gyro[4] = z_gyro;
+}
+
+


Cerveau: gyroscope - ajout du calcul des angles à partir de l'accélération

error: pathspec 'Module' did not match any file(s) known to git
error: pathspec 'Cerveau/Gyroscope/Gyroscope/Gyroscope/Gyroscope.ino' did not match any file(s) known to git


Initial Commit

Initial Commit

--- Encrypt/Encrypt.ino
+++ Encrypt/Encrypt.ino
@@ -0,0 +1,35 @@
+
+void setup()
+{
+  // Ini ports
+
+  Serial.begin(9600);
+  Serial3.begin(9600);
+
+}
+
+int16_t key = 100;
+
+void loop()
+{
+  // read from port 3
+  if(Serial3.available())
+  {
+    int16_t inByte = Serial3.read();
+
+    int16_t decByte = (inByte - key)%256;
+
+    Serial.write(decByte);
+  }
+
+  // read from serial moniter
+  if(Serial.available())
+  {
+    int16_t inByte = Serial.read();
+
+    int16_t encByte = (inByte + key)%256;
+
+    Serial3.write(encByte);
+  }
+}
+


Add canbus FD send example

Add canbus FD send example

error: pathspec 'examples/sendFD/sendFD.ino' did not match any file(s) known to git

--- examples/sendFD/sendFD.ino
+++ examples/sendFD/sendFD.ino
@@ -0,0 +1,58 @@
+// demo: CAN-BUS Shield, send data
+// loovee@seeed.cc
+
+
+#include <SPI.h>
+#include "mcp2518fd_can.h"
+
+/*SAMD core*/
+#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
+    #define SERIAL SerialUSB
+#else
+    #define SERIAL Serial
+#endif
+
+
+#define CAN_2518FD
+// the cs pin of the version after v1.1 is default to D9
+// v0.9b and v1.0 is default D10
+const int SPI_CS_PIN = BCM8;
+
+#ifdef CAN_2518FD
+mcp2518fd CAN(SPI_CS_PIN); // Set CS pin
+#endif
+
+void setup() {
+    SERIAL.begin(115200);
+    while(!Serial){};
+    CAN.setMode(0);
+    while (0 != CAN.begin((byte)CAN_500K_1M)) {            // init can bus : baudrate = 500k
+        SERIAL.println("CAN BUS Shield init fail");
+        SERIAL.println(" Init CAN BUS Shield again");
+        delay(100);
+    }
+    byte mode = CAN.getMode();
+    SERIAL.printf("CAN BUS get mode = %d\n\r",mode);
+    SERIAL.println("CAN BUS Shield init ok!");
+}
+
+unsigned char stmp[64] = {0};
+void loop() {
+    // send data:  id = 0x00, standrad frame, data len = 8, stmp: data buf
+    stmp[63] = stmp[63] + 1;
+    if (stmp[63] == 100) {
+        stmp[63] = 0;
+        stmp[63] = stmp[63] + 1;
+
+        if (stmp[6] == 100) {
+            stmp[6] = 0;
+            stmp[5] = stmp[6] + 1;
+        }
+    }
+
+    CAN.sendMsgBuf(0x00, 0, 15, stmp);
+    delay(100);                       // send data per 100ms
+    SERIAL.println("CAN BUS sendMsgBuf ok!");
+}
+
+// END FILE


add arduino program

add arduino program

remote: Support for password authentication was removed on August 13, 2021.
remote: Please see https://docs.github.com/en/get-started/getting-started-with-git/about-remote-repositories#cloning-with-https-urls for information on currently recommended modes of authentication.
fatal: Authentication failed for 'https://github.com/bloc11/authsys-common.git/'