API tools faq
paste
Advertisement
Guest User

Cheap VR Glasses -- ongoing -- Pierre Igor Z.

a guest
Sep 29th, 2020
384
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 37.34 KB | None | 0 0
raw download clone embed print report
  1. #include <SPI.h>
  2. #include <Wire.h>
  3. #include <Adafruit_GFX.h>
  4. #include <Adafruit_SSD1306.h>
  5.  
  6. /*************************************************************************
  7.    DECLARE MAIN
  8. */
  9. #define OLED_RESET 4
  10. Adafruit_SSD1306 display(OLED_RESET);
  11. Adafruit_SSD1306 display2(OLED_RESET);
  12.  
  13. #define XPOS 0
  14. #define YPOS 1
  15. #define DELTAY 2
  16.  
  17. #define LOGO16_GLCD_HEIGHT 16
  18. #define LOGO16_GLCD_WIDTH  16
  19. static const unsigned char PROGMEM logo16_glcd_bmp[] =
  20. { B00000000, B00000100,
  21.   B00000000, B00011000,
  22.   B00000000, B01110000,
  23.   B00000001, B11100000,
  24.   B00000011, B11000000,
  25.   B00000111, B10000000,
  26.   B00000111, B10000000,
  27.   B00000011, B11000000,
  28.   B00011111, B11111000,
  29.   B00001111, B11111000,
  30.   B00000001, B11100000,
  31.   B00000011, B11000000,
  32.   B00000111, B10000000,
  33.   B00011110, B00000000,
  34.   B01110000, B00000000,
  35.   B00000000, B00000000
  36. };
  37.  
  38. #if (SSD1306_LCDHEIGHT != 64)
  39. #error("Height incorrect");
  40. #endif
  41.  
  42. int incomingByte = 0;
  43.  
  44. byte menu = 0;
  45. byte menuSelect = 0;
  46. #define menuOffset 16
  47.  
  48. bool dispVR = true;
  49.  
  50. float eyeSocket[4];
  51. float eyeDirection[4];
  52. float eyeUp[4];
  53.  
  54. float worldMat[16];
  55. float worldMat2[16];
  56. float rollMat[16];
  57. float rollMat2[16];
  58. float tmpMat[16];
  59.  
  60. float clipWidth = 2.0; //change that to zoom
  61. float clipHeight = 1.0;
  62. /*float halfClipWidth = clipWidth / 2.0;
  63.   float halfClipHeight = clipHeight / 2.0;*/
  64. float Plocal[4];
  65. float Pworld[4];
  66. float Pcamera[4];
  67. float Pscreen[4];
  68. float Pnds[2];
  69. byte Ppix[2];
  70.  
  71. int timeOffset;
  72. int startOffset;
  73.  
  74. float pitch = 0.0;
  75. float yaw = 0.0;
  76. float roll = 0.0;
  77.  
  78. bool didOnce = false;
  79. bool showKeyb = false;
  80. bool showDebug = false;
  81.  
  82. /******************************************************************************
  83.    DECLARE IMU
  84. */
  85.  
  86. // "MPU-9150 Register Map and Descriptions Revision 4.0",
  87.  
  88. #define MPU9150_SELF_TEST_X        0x0D   // R/W
  89. #define MPU9150_SELF_TEST_Y        0x0E   // R/W
  90. #define MPU9150_SELF_TEST_X        0x0F   // R/W
  91. #define MPU9150_SELF_TEST_A        0x10   // R/W
  92. #define MPU9150_SMPLRT_DIV         0x19   // R/W
  93. #define MPU9150_CONFIG             0x1A   // R/W
  94. #define MPU9150_GYRO_CONFIG        0x1B   // R/W
  95. #define MPU9150_ACCEL_CONFIG       0x1C   // R/W
  96. #define MPU9150_FF_THR             0x1D   // R/W
  97. #define MPU9150_FF_DUR             0x1E   // R/W
  98. #define MPU9150_MOT_THR            0x1F   // R/W
  99. #define MPU9150_MOT_DUR            0x20   // R/W
  100. #define MPU9150_ZRMOT_THR          0x21   // R/W
  101. #define MPU9150_ZRMOT_DUR          0x22   // R/W
  102. #define MPU9150_FIFO_EN            0x23   // R/W
  103. #define MPU9150_I2C_MST_CTRL       0x24   // R/W
  104. #define MPU9150_I2C_SLV0_ADDR      0x25   // R/W
  105. #define MPU9150_I2C_SLV0_REG       0x26   // R/W
  106. #define MPU9150_I2C_SLV0_CTRL      0x27   // R/W
  107. #define MPU9150_I2C_SLV1_ADDR      0x28   // R/W
  108. #define MPU9150_I2C_SLV1_REG       0x29   // R/W
  109. #define MPU9150_I2C_SLV1_CTRL      0x2A   // R/W
  110. #define MPU9150_I2C_SLV2_ADDR      0x2B   // R/W
  111. #define MPU9150_I2C_SLV2_REG       0x2C   // R/W
  112. #define MPU9150_I2C_SLV2_CTRL      0x2D   // R/W
  113. #define MPU9150_I2C_SLV3_ADDR      0x2E   // R/W
  114. #define MPU9150_I2C_SLV3_REG       0x2F   // R/W
  115. #define MPU9150_I2C_SLV3_CTRL      0x30   // R/W
  116. #define MPU9150_I2C_SLV4_ADDR      0x31   // R/W
  117. #define MPU9150_I2C_SLV4_REG       0x32   // R/W
  118. #define MPU9150_I2C_SLV4_DO        0x33   // R/W
  119. #define MPU9150_I2C_SLV4_CTRL      0x34   // R/W
  120. #define MPU9150_I2C_SLV4_DI        0x35   // R  
  121. #define MPU9150_I2C_MST_STATUS     0x36   // R
  122. #define MPU9150_INT_PIN_CFG        0x37   // R/W
  123. #define MPU9150_INT_ENABLE         0x38   // R/W
  124. #define MPU9150_INT_STATUS         0x3A   // R  
  125. #define MPU9150_ACCEL_XOUT_H       0x3B   // R  
  126. #define MPU9150_ACCEL_XOUT_L       0x3C   // R  
  127. #define MPU9150_ACCEL_YOUT_H       0x3D   // R  
  128. #define MPU9150_ACCEL_YOUT_L       0x3E   // R  
  129. #define MPU9150_ACCEL_ZOUT_H       0x3F   // R  
  130. #define MPU9150_ACCEL_ZOUT_L       0x40   // R  
  131. #define MPU9150_TEMP_OUT_H         0x41   // R  
  132. #define MPU9150_TEMP_OUT_L         0x42   // R  
  133. #define MPU9150_GYRO_XOUT_H        0x43   // R  
  134. #define MPU9150_GYRO_XOUT_L        0x44   // R  
  135. #define MPU9150_GYRO_YOUT_H        0x45   // R  
  136. #define MPU9150_GYRO_YOUT_L        0x46   // R  
  137. #define MPU9150_GYRO_ZOUT_H        0x47   // R  
  138. #define MPU9150_GYRO_ZOUT_L        0x48   // R  
  139. #define MPU9150_EXT_SENS_DATA_00   0x49   // R  
  140. #define MPU9150_EXT_SENS_DATA_01   0x4A   // R  
  141. #define MPU9150_EXT_SENS_DATA_02   0x4B   // R  
  142. #define MPU9150_EXT_SENS_DATA_03   0x4C   // R  
  143. #define MPU9150_EXT_SENS_DATA_04   0x4D   // R  
  144. #define MPU9150_EXT_SENS_DATA_05   0x4E   // R  
  145. #define MPU9150_EXT_SENS_DATA_06   0x4F   // R  
  146. #define MPU9150_EXT_SENS_DATA_07   0x50   // R  
  147. #define MPU9150_EXT_SENS_DATA_08   0x51   // R  
  148. #define MPU9150_EXT_SENS_DATA_09   0x52   // R  
  149. #define MPU9150_EXT_SENS_DATA_10   0x53   // R  
  150. #define MPU9150_EXT_SENS_DATA_11   0x54   // R  
  151. #define MPU9150_EXT_SENS_DATA_12   0x55   // R  
  152. #define MPU9150_EXT_SENS_DATA_13   0x56   // R  
  153. #define MPU9150_EXT_SENS_DATA_14   0x57   // R  
  154. #define MPU9150_EXT_SENS_DATA_15   0x58   // R  
  155. #define MPU9150_EXT_SENS_DATA_16   0x59   // R  
  156. #define MPU9150_EXT_SENS_DATA_17   0x5A   // R  
  157. #define MPU9150_EXT_SENS_DATA_18   0x5B   // R  
  158. #define MPU9150_EXT_SENS_DATA_19   0x5C   // R  
  159. #define MPU9150_EXT_SENS_DATA_20   0x5D   // R  
  160. #define MPU9150_EXT_SENS_DATA_21   0x5E   // R  
  161. #define MPU9150_EXT_SENS_DATA_22   0x5F   // R  
  162. #define MPU9150_EXT_SENS_DATA_23   0x60   // R  
  163. #define MPU9150_MOT_DETECT_STATUS  0x61   // R  
  164. #define MPU9150_I2C_SLV0_DO        0x63   // R/W
  165. #define MPU9150_I2C_SLV1_DO        0x64   // R/W
  166. #define MPU9150_I2C_SLV2_DO        0x65   // R/W
  167. #define MPU9150_I2C_SLV3_DO        0x66   // R/W
  168. #define MPU9150_I2C_MST_DELAY_CTRL 0x67   // R/W
  169. #define MPU9150_SIGNAL_PATH_RESET  0x68   // R/W
  170. #define MPU9150_MOT_DETECT_CTRL    0x69   // R/W
  171. #define MPU9150_USER_CTRL          0x6A   // R/W
  172. #define MPU9150_PWR_MGMT_1         0x6B   // R/W
  173. #define MPU9150_PWR_MGMT_2         0x6C   // R/W
  174. #define MPU9150_FIFO_COUNTH        0x72   // R/W
  175. #define MPU9150_FIFO_COUNTL        0x73   // R/W
  176. #define MPU9150_FIFO_R_W           0x74   // R/W
  177. #define MPU9150_WHO_AM_I           0x75   // R
  178.  
  179. //MPU9150 Compass
  180. #define MPU9150_CMPS_XOUT_L        0x4A   // R
  181. #define MPU9150_CMPS_XOUT_H        0x4B   // R
  182. #define MPU9150_CMPS_YOUT_L        0x4C   // R
  183. #define MPU9150_CMPS_YOUT_H        0x4D   // R
  184. #define MPU9150_CMPS_ZOUT_L        0x4E   // R
  185. #define MPU9150_CMPS_ZOUT_H        0x4F   // R
  186.  
  187.  
  188. // I2C address 0x69 could be 0x68 depends on your wiring.
  189. int MPU9150_I2C_ADDRESS = 0x69;
  190.  
  191.  
  192. //Variables where our values can be stored
  193. int cmps[3];
  194. int accl[3];
  195. int gyro[3];
  196. int temp;
  197.  
  198.  
  199. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////SETUP
  200. /***
  201.    FUNCTIONS MAIN
  202. */
  203. void setup()   {
  204.   Serial.begin(115200);
  205.   display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  206.   display2.begin(SSD1306_SWITCHCAPVCC, 0x3D);
  207.   display.clearDisplay();
  208.   display2.clearDisplay();
  209.   display2.display();
  210.  
  211.  
  212.  
  213.   int i, j;
  214.   for (i = 0; i < 4; ++i)
  215.     for (j = 0; j < 4; ++j)
  216.       if (i == j) worldMat[i] = rollMat[i] = rollMat2[i] = tmpMat[i] = 1;
  217.       else worldMat[i] = rollMat[i] = rollMat2[i] = tmpMat[i] = 0;
  218.  
  219.   //worldMat[0] = worldMat[4 + 1] = worldMat[8 + 2] = worldMat[12 + 3] = 1; //Identity world
  220.  
  221.   eyeSocket[0] = 0.0;
  222.   eyeSocket[1] = -1.8;
  223.   eyeSocket[2] = 0.0;
  224.   eyeSocket[3] = 1.0;
  225.  
  226.   eyeDirection[0] = 0.0;
  227.   eyeDirection[1] = 0.0;
  228.   eyeDirection[2] = -1.0;
  229.   eyeDirection[3] = 1.0;
  230.  
  231.   eyeUp[0] = 0; eyeUp[2] = 0;
  232.   eyeUp[1] = 1; eyeUp[3] = 1;
  233.  
  234.   /*
  235.     float ca = cos(yaw);
  236.     float cb = cos(roll);
  237.     float cy = cos(pitch); //camera relative pitch/yaw/roll
  238.     float sa = sin(yaw);
  239.     float sb = sin(roll);
  240.     float sy = sin(pitch);
  241.     rollMat[0] = ca * cb;
  242.     rollMat[1] = ca * sb * sy - sa * cy;
  243.     rollMat[2] = ca * sb * cy + sa * sy;
  244.     rollMat[3] = 0;
  245.     rollMat[4] = sa * cb;
  246.     rollMat[5] = sa * sb * sy + ca * cy;
  247.     rollMat[6] = sa * sb * cy - ca * sy;
  248.     rollMat[7] = 0;
  249.     rollMat[8] = -sb;
  250.     rollMat[9] = cb * sy;
  251.     rollMat[10] = cb * cy;
  252.     rollMat[11] = 0;
  253.     rollMat[12] = rollMat[13] = rollMat[14] = 0;
  254.     rollMat[15] = 1;*/
  255.  
  256.   /////OTHER SETUP
  257.   setupIMU();
  258.   startOffset = millis();
  259.  
  260.   Serial.println(F("Setup."));
  261.  
  262.   displayMenu(0);
  263. }
  264.  
  265. void displayMenu(int menuID) {
  266.   display.clearDisplay();
  267.   display.setTextSize(1);
  268.   display.setTextColor(BLACK, WHITE);
  269.   display.setCursor(0, 0);
  270.   //display.display();
  271.  
  272.   switch (menuID) {
  273.     case 0: display.println(F("  freecomm OS  "));
  274.       break;
  275.     case 1: display.println(F("  VR Metaverse  "));
  276.       break; // hide menu in metaverse
  277.     case 2: display.println(F("  Messenger  "));
  278.       break;
  279.     case 3: display.println(F("  Music  "));
  280.       break;
  281.     case 4: display.println(F("  Other  "));
  282.       break;
  283.     case 5: display.println(F("  Settings  "));
  284.       break;
  285.     case 6: display.println(F("  Apps  "));
  286.       break;
  287.     default: display.println(F("  freecomm OS  "));
  288.       break;
  289.   }
  290.  
  291.  
  292.   display.println(F("wat"));
  293.   display.setTextColor(WHITE);
  294.   displayMenuItems(menuID);
  295.  
  296.  
  297.  
  298.   display.setCursor(0, menuOffset + menuSelect * 8);
  299.   display.print("*");
  300.   display.drawBitmap(100, 10,  logo16_glcd_bmp, 16, 16, 1);
  301.  
  302.  
  303. }
  304.  
  305. void displayMenuItems(int menuID) {
  306.   switch (menuID) {
  307.     case 0: //Main menu
  308.       display.println(F("  VR Metaverse"));
  309.       display.println(F("  Messenger"));
  310.       display.println(F("  Music"));
  311.       display.println(F("  Other"));
  312.       display.println(F("  Settings"));
  313.       break;
  314.     case 1: //Metaverse menu
  315.       display.println(F("  Summon Object"));
  316.       display.println(F("  Invite Peer"));
  317.       display.println(F("  Teleport"));
  318.       display.println(F("  Settings"));
  319.       display.println(F("  Exit"));
  320.       break; // no header in metaverse
  321.     case 2: //Messenger menu
  322.       display.println(F("  New (0)"));
  323.       display.println(F("  Send"));
  324.       display.println(F("  Inbox"));
  325.       display.println(F("  Outbox"));
  326.       display.println(F("  Exit"));
  327.       break;
  328.     case 3: //Music menu
  329.       display.println(F("  Listen"));
  330.       display.println(F("  Play"));
  331.       display.println(F("  Exit"));
  332.       break;
  333.     case 4: //Other menu
  334.       display.println(F("  Wiki  "));
  335.       display.println(F("  Apps  "));
  336.       display.println(F("  Files  "));
  337.       display.println(F("  Stream  "));
  338.       display.println(F("  Exit  "));
  339.       break;
  340.     case 5: //Settings menu
  341.       display.println(F("  General  "));
  342.       display.println(F("  Display  "));
  343.       display.println(F("  Memory  "));
  344.       display.println(F("  Network  "));
  345.       display.println(F("  Reset  "));
  346.       display.println(F("  Exit  "));
  347.       break;
  348.     case 6: //Apps menu
  349.       //Here we should load Apps from memory
  350.       display.println(F("  Learn Chinese  "));
  351.       break;
  352.     default: display.println(F("  Exit  ")); //go to Main
  353.       break;
  354.   }
  355. }
  356.  
  357. void loop() {
  358.   if (Serial.available() > 0) {
  359.     // read the incoming byte:
  360.     incomingByte = Serial.read();
  361.     while (incomingByte != -1) {
  362.       // say what you got:
  363.       Serial.println(incomingByte, DEC);
  364.       //display.write(incomingByte);
  365.       if (incomingByte == '1')
  366.         menu = 1;
  367.       switch (incomingByte) {
  368.         case 'j': menuSelect++; if (menuSelect > 6) menuSelect = 0; break; //up button
  369.         case 'h': menuSelect--; if (menuSelect == -1) menuSelect = 0; break; //down button
  370.         case 'g': select(menuSelect); break; //select button
  371.         case 'e': dispVR = false; menu = 0; menuSelect = 0; break; //home button
  372.  
  373.         case 'r': goRoll(-0.1); break;
  374.         case 't': goRoll(0.1); break;
  375.         case 'x': goYaw(-0.1); break; ////yaw -= 0.1; break;
  376.         case 'c': goYaw(0.1); break;
  377.         case 'f': goPitch(-0.1); break;
  378.         case 'v': goPitch(0.1); break;
  379.  
  380.         /*case 'r': roll -= 0.1; break;
  381.           case 't': roll += 0.1; break;
  382.           case 'd': yaw -= 0.1; break; ////yaw -= 0.1; break;
  383.           case 'c': yaw += 0.1; break;
  384.           case 'f': pitch -= 0.1; break;
  385.           case 'v': pitch += 0.1; break;*/
  386.  
  387.         case 'z': goForward(1.0); break;
  388.         case 's': goBackward(1.0); break;
  389.         case 'q': goLeft(1.0); break;
  390.         case 'd': goRight(1.0); break;
  391.         case '5': eyeSocket[0] = eyeSocket[2] = 0; eyeSocket[1] = -1.8; yaw = pitch = roll = 0; break;
  392.         case '8': /*eyeSocket[2]--; */ goForward(1); break;
  393.         case '2': eyeSocket[2]++; break;
  394.         case '4': eyeSocket[0]++; break;
  395.         case '6': eyeSocket[0]--; break;
  396.  
  397.         case 'k': showKeyb = !showKeyb; break;
  398.  
  399.         case 'i': displayStream(); break; //serial stream to display --sends what comes after i, until newline
  400.  
  401.         default: break; // store input for processing
  402.       }
  403.       incomingByte = Serial.read();
  404.     }
  405.   }
  406.  
  407.   readGyroYaw();
  408.   readAccelPitch();
  409.   readAccelRoll();
  410.  
  411.  
  412.   if (dispVR)
  413.     displayVR();
  414.   else
  415.     displayMenu(menu);
  416.  
  417.   if (showKeyb)
  418.     displayKeyb();
  419.  
  420.   display.display();
  421.   display2.display();
  422.  
  423.   //OTHER LOOP
  424.   if (false)
  425.     loopIMU();
  426.  
  427.   delay(30);
  428. }
  429.  
  430. void readAccelPitch() {
  431.   //double accelX = MPU9150_readSensor(MPU9150_ACCEL_XOUT_L, MPU9150_ACCEL_XOUT_H);
  432.   //double accelY = MPU9150_readSensor(MPU9150_ACCEL_YOUT_L, MPU9150_ACCEL_YOUT_H);
  433.   double accelZ = MPU9150_readSensor(MPU9150_ACCEL_ZOUT_L, MPU9150_ACCEL_ZOUT_H);
  434.  
  435.   //de z = 15000 (bas) à z = -15000 (haut), soit en radian -pi/2 à pi/2
  436.   //map(
  437.   double pitchRad = accelZ / 10000.0;
  438.   if (pitchRad < -1.57) pitchRad = -1.57;
  439.   if (pitchRad > 1.57) pitchRad = 1.57;
  440.   goPitchAbsolute(-pitchRad);
  441.  
  442.   if (showDebug) {
  443.     Serial.print("Pitch: ");
  444.     Serial.println(pitchRad);  
  445.   }
  446.   //pitch = pitchRad;
  447.  
  448. }
  449.  
  450. void readAccelRoll() {
  451.   //double accelX = MPU9150_readSensor(MPU9150_ACCEL_XOUT_L, MPU9150_ACCEL_XOUT_H);
  452.   //double accelY = MPU9150_readSensor(MPU9150_ACCEL_YOUT_L, MPU9150_ACCEL_YOUT_H);
  453.   double accelY = MPU9150_readSensor(MPU9150_ACCEL_YOUT_L, MPU9150_ACCEL_YOUT_H);
  454.  
  455.   //de z = 15000 (bas) à z = -15000 (haut), soit en radian -pi/2 à pi/2
  456.   //map(
  457.   double rollRad = accelY / 10000.0;
  458.   if (rollRad < -1.57) rollRad = -1.57;
  459.   if (rollRad > 1.57) rollRad = 1.57;
  460.   goRollAbsolute(rollRad);
  461.  
  462.   if (showDebug) {
  463.     Serial.print("Roll: ");
  464.     Serial.println(rollRad);  
  465.   }
  466.   //roll = rollRad;
  467.  
  468. }
  469.  
  470. void readGyroYaw() {
  471.   double gyroX = MPU9150_readSensor(MPU9150_GYRO_XOUT_L, MPU9150_GYRO_XOUT_H);
  472.   double gyroY = MPU9150_readSensor(MPU9150_GYRO_YOUT_L, MPU9150_GYRO_YOUT_L);
  473.   double gyroZ = MPU9150_readSensor(MPU9150_GYRO_ZOUT_L, MPU9150_GYRO_ZOUT_H);
  474.  
  475.   Serial.print("Gyro: "); Serial.print(gyroX); Serial.print("/"); Serial.print(gyroY);Serial.print("/");Serial.println(gyroZ);
  476.   float gyroToYaw;
  477.   gyroToYaw = gyroX / 15000;
  478.   //Donc il faut simplement réduire gyroToYaw en fonction de roll, et ça serait un bon début
  479.   gyroToYaw = gyroToYaw * cos(roll);
  480.   gyroToYaw = -gyroToYaw;
  481.   gyroToYaw += yaw;
  482.  
  483.   Serial.print("GyroToYaw: "); Serial.println(gyroToYaw);
  484.  
  485.   goYawAbsolute(-gyroToYaw);
  486. }
  487.  
  488. /*void printtab(char* text) {
  489.   Serial.print
  490. }*/
  491.  
  492. void displayStream() {
  493.   bool goOn = true;
  494.   byte streamByte;
  495.   Serial.println(F("displaying stream"));
  496.   display.clearDisplay();
  497.   int i;
  498.   int pixelPos = 0;
  499.   int maxPixelPos = 128 * 64 + 127; //todo: selon taille de l'écran
  500.  
  501.   /*Serial.end();
  502.     delay(5000);
  503.     Serial.begin(115200);*/
  504.  
  505.   while (goOn) {
  506.     if (goOn && Serial.available() > 0) {
  507.       // read the incoming byte:
  508.       incomingByte = (int) (streamByte = Serial.read());
  509.       /*Serial.print(F("stream read "));
  510.         Serial.println(streamByte);*/
  511.       if (streamByte == 111)
  512.         goOn = false;
  513.  
  514.       while (goOn && streamByte != -1 && streamByte != 255) {
  515.         for (i = 0; i < 8; ++i) {
  516.           display.drawPixel(pixelPos % 128, pixelPos / 128, (streamByte & (128 >> i)) ? WHITE : BLACK); //todo: replace 128 with display width
  517.           //todo: diviser selon taille de l'écran
  518.           ++pixelPos;
  519.           if (pixelPos > maxPixelPos) pixelPos = 0;
  520.         }
  521.  
  522.         incomingByte = (int) (streamByte = Serial.read());
  523.       }
  524.     }
  525.     display.display();
  526.     delay(100);
  527.   }
  528.  
  529.   incomingByte = -1;
  530.   //Serial.end();
  531.   //Serial.begin(9600);
  532. }
  533.  
  534. void displayApps() {
  535.  
  536. }
  537.  
  538. /*
  539.   void downloadStream(int dlAddr, int dlSize) {
  540.  
  541.   }
  542.  
  543.   void uploadStream(int ulAddr, int ulSize) {
  544.  
  545.   }
  546. */
  547. void displayKeyb() {
  548.   display.setCursor(0, 10);
  549.   display.println(F("buffer"));
  550.   display.println();
  551.   display.println(F("1 2 3 4 5 6 7 8 9 0"));
  552.   display.println(F("q w e r t y u i o p"));
  553.   display.println(F("a s d f g h j k l ."));
  554.   display.println(F("z x c v b n m ; : !"));
  555. }
  556.  
  557. void select(int menuSel) {
  558.   switch (menu) {
  559.     case 0:
  560.       switch (menuSel) {
  561.         case 0: dispVR = true; menu = 1; menuSelect = 0; break; //go metaverse
  562.         case 1: menu = 2; menuSelect = 0; break; //Messenger
  563.         case 2: menu = 3; menuSelect = 0; break; //Music
  564.         case 3: menu = 4; menuSelect = 0; break; //Other
  565.         case 4: menu = 5; menuSelect = 0; break; //Settings
  566.         default: break;
  567.       } break;
  568.     case 1:  //VR
  569.       switch (menuSel) {
  570.         case 4: menu = 0; menuSelect = 0; break;
  571.         default: break;
  572.       } break;
  573.     case 2:
  574.       switch (menuSel) {
  575.         case 4: menu = 0; menuSelect = 0; break;
  576.         default: break;
  577.       } break;
  578.     case 3:  
  579.       switch (menuSel) {
  580.         case 2: menu = 0; menuSelect = 0; break;
  581.         default: break;
  582.       } break;
  583.     case 4:
  584.       switch (menuSel) {
  585.         case 1: menu = 6; menuSelect = 0; break; //throw event here? //Apps
  586.         case 3: displayStream(); break; //serial stream to display --sends what comes until user presses o
  587.         case 4: menu = 0; menuSelect = 0; break;
  588.         default: break;
  589.       } break;
  590.     case 5:
  591.       switch (menuSel) {
  592.         case 5: menu = 0; menuSelect = 0; break;
  593.         default: break;
  594.       } break;
  595.     case 6:
  596.       /*switch (menuSel) {
  597.         Here we should have loaded the list of App IDs, and depending on menuSel, launch the corresponding app dynamically
  598.       }*/
  599.       switch (menuSel) {
  600.         case 0: learnChineseApp(); break;
  601.       }
  602.       break;
  603.     default:
  604.       menu = 0; menuSelect = 0; break;
  605.   }
  606. }
  607. void goForward(float speed) {
  608.   int i;
  609.   for (i = 0; i < 3; ++i)
  610.     eyeSocket[i] += eyeDirection[i] * speed;
  611. }
  612. void goBackward(float speed) {
  613.   int i;
  614.   for (i = 0; i < 3; ++i)
  615.     eyeSocket[i] -= eyeDirection[i] * speed;
  616. }
  617. void goLeft(float speed) {
  618.   int i;
  619.   float camDir[4];
  620.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  621.   for (i = 0; i < 3; ++i)
  622.     eyeSocket[i] += camDir[i] * speed;
  623. }
  624. void goRight(float speed) {
  625.   int i;
  626.   float camDir[4];
  627.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  628.   for (i = 0; i < 3; ++i)
  629.     eyeSocket[i] -= camDir[i] * speed;
  630. }
  631.  
  632. void goYaw(float ang) {
  633.   int i;
  634.   float camDir[4];
  635.   for (i = 0; i < 4; ++i) camDir[i] = eyeDirection[i];
  636.   transformFromAxisAngle(tmpMat, eyeUp, ang);
  637.   multiplication(tmpMat, camDir, eyeDirection, 4, 4, 4, 1);
  638.   // maybe null &ang
  639. }
  640.  
  641. void goRoll(float ang) {
  642.   int i;
  643.   float camDir[4];
  644.   for (i = 0; i < 4; ++i) camDir[i] = eyeUp[i];
  645.   transformFromAxisAngle(tmpMat, eyeDirection, ang);
  646.   multiplication(tmpMat, camDir, eyeUp, 4, 4, 4, 1);
  647.   roll += ang;
  648. }
  649.  
  650. //this is not the pitch I want, but it is good to use for Gyro X
  651. void goPitch(float ang) {
  652.   int i;
  653.   float camDir[4];
  654.   //for (i = 0; i < 4; ++i) camDir[i] = eyeDirection[i];
  655.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  656.   camDir[3] = 1;
  657.   normalize3(camDir);
  658.   transformFromAxisAngle(tmpMat, camDir, ang);
  659.   for (i = 0; i < 4; ++i) camDir[i] = eyeDirection[i];
  660.   multiplication(tmpMat, camDir, eyeDirection, 4, 4, 4, 1);
  661.   //normalize3(eyeDirection);
  662.  
  663.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  664.   camDir[3] = 1;
  665.   normalize3(camDir);
  666.   transformFromAxisAngle(tmpMat, camDir, ang);
  667.   for (i = 0; i < 4; ++i) camDir[i] = eyeUp[i];
  668.   multiplication(tmpMat, camDir, eyeUp, 4, 4, 4, 1);
  669.   //normalize3(eyeUp);
  670.  
  671.   /*for (i = 0; i < 4; ++i) camDir[i] = eyeUp[i];
  672.     multiplication(tmpMat, camDir, eyeUp, 4, 4, 4, 1);*/
  673.   //Full Pitch; disable for different effect with fixed eyeup
  674.   /*
  675.  
  676.     Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  677.     camDir[3] = 1;
  678.     normalize3(camDir);
  679.     transformFromAxisAngle(tmpMat, camDir, ang);
  680.     for (i = 0; i < 4; ++i) camDir[i] = eyeUp[i];
  681.     multiplication(tmpMat, camDir, eyeUp, 4, 4, 4, 1);*/
  682.     pitch += ang;
  683. }
  684.  
  685. //void goPitchSimple(angle
  686.  
  687. void goPitchAbsolute(float ang) {
  688.   //What the fuck.
  689.   /*
  690.   int i;
  691.   float camDir[4];
  692.   //for (i = 0; i < 4; ++i) camDir[i] = eyeDirection[i];
  693.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  694.   camDir[3] = 1;
  695.   normalize3(camDir);
  696.   transformFromAxisAngle(tmpMat, camDir, ang);
  697.   for (i = 0; i < 4; ++i) camDir[i] = eyeDirection[i];
  698.   multiplication(tmpMat, camDir, eyeDirection, 4, 4, 4, 1);
  699.   //normalize3(eyeDirection);
  700.  
  701.   Vector_Cross_Product(camDir, eyeUp, eyeDirection);
  702.   camDir[3] = 1;
  703.   normalize3(camDir);
  704.   transformFromAxisAngle(tmpMat, camDir, ang);
  705.   for (i = 0; i < 4; ++i) camDir[i] = eyeUp[i];
  706.   multiplication(tmpMat, camDir, eyeUp, 4, 4, 4, 1);
  707.   */
  708.     //WTF- On va juste enlever le dernier pitch, et remettre le nouveau
  709.     goPitch(-pitch);
  710.     goPitch(ang);
  711. }
  712.  
  713. void goRollAbsolute(float ang) {
  714.   //What the fuck.
  715.     goRoll(-roll);
  716.     goRoll(ang);
  717. }
  718.  
  719. void goYawAbsolute(float ang) {
  720.   //What the fuck.
  721.     goYaw(-yaw);
  722.     goYaw(ang);
  723. }
  724.  
  725. void displayVR() {
  726.   display.clearDisplay();
  727.   display2.clearDisplay();
  728.   int i, j;
  729.   /*worldMat[0] = cos(pitch);
  730.     worldMat[2] = sin(pitch);
  731.     worldMat[8] = -sin(pitch);
  732.     worldMat[10] = cos(pitch);*/
  733.  
  734.   for (i = 0; i < 4; ++i)
  735.     for (j = 0; j < 4; ++j)
  736.       if (i == j) tmpMat[i * 4 + j] = 1;
  737.       else tmpMat[i * 4 + j] = 0;
  738.  
  739.   /*tmpMat[3] = -eyeSocket[0];
  740.     tmpMat[7] = -eyeSocket[1];
  741.     tmpMat[11] = -eyeSocket[2];
  742.     tmpMat[15] = 1;*/
  743.  
  744.   //We should compute eyeDir here out of roll/pitch/yaw, and use that as rollMat
  745.   //order is roll pitch yaw, or reverse
  746.   //roll should create eyeUp, and yaw/pitch should create eyeDir. Yaw first, then pitch, to the eyeDir vector, with transformFromAng
  747.   //void transformFromAxisAngle(float vecOut[], float v[], float a) {
  748.   float newEyeUp[4];
  749.   for (i = 0; i < 4; ++i) newEyeUp[4] = eyeUp[4];
  750.   //cela depend de eyeDir. On était sur l'axe Z, mais on va tourner dans X/Y au sacrifice de Z.
  751.  
  752.   //First eye
  753.   float eyeDir[4];
  754.   for (i = 0; i < 4; ++i) {
  755.     eyeDir[i] = eyeSocket[i] + eyeDirection[i]; //* //speed
  756.   }
  757.   createLookAt(eyeSocket, eyeDir, eyeUp, rollMat);
  758.  
  759.   //Second eye
  760.   float eyeRight[4];
  761.   Vector_Cross_Product(eyeRight, eyeDirection, eyeUp); //should be eyeDirection, not eyeDir, as eyeDir isn't even unitary
  762.   //matrixScalarMult(eyeRight, 3, 3.0); //eye separation
  763.   eyeRight[4] = 1.0;
  764.  
  765.   float eyeDir2[4];
  766.   for (i = 0; i < 4; ++i)
  767.     eyeDir2[i] = eyeDir[i] + eyeRight[i];
  768.  
  769.   for (i = 0; i < 4; ++i)
  770.     eyeRight[i] += eyeSocket[i];
  771.  
  772.   createLookAt(eyeRight/*todo: * eyeSeparation*/, eyeDir2, eyeUp, rollMat2);
  773.  
  774.   //createLookAt(tmpMat, eyeDir, eyeUp, rollMat);
  775.  
  776.   /*
  777.     float ca = cos(roll);
  778.     float sa = sin(roll);
  779.     float cy = cos(pitch); //camera relative pitch/yaw/roll
  780.     float sy = sin(pitch); //on a inversé pitch et yaw
  781.     float cb = cos(yaw);
  782.     float sb = sin(yaw);
  783.  
  784.     rollMat[0] = ca * cb;
  785.     rollMat[1] = ca * sb * sy - sa * cy;
  786.     rollMat[2] = ca * sb * cy + sa * sy;
  787.     rollMat[3] = 0;
  788.     rollMat[4] = sa * cb;
  789.     rollMat[5] = sa * sb * sy + ca * cy;
  790.     rollMat[6] = sa * sb * cy - ca * sy;
  791.     rollMat[7] = 0;
  792.     rollMat[8] = -sb;
  793.     rollMat[9] = cb * sy;
  794.     rollMat[10] = cb * cy;
  795.     rollMat[11] = 0;
  796.     rollMat[12] = rollMat[13] = rollMat[14] = 0;
  797.     rollMat[15] = 1; 5h du mat' last change*/
  798.  
  799.   tmpMat[3] = -eyeSocket[0];
  800.   tmpMat[7] = -eyeSocket[1];
  801.   tmpMat[11] = -eyeSocket[2];
  802.   tmpMat[15] = 1;
  803.   multiplication(rollMat, tmpMat, worldMat, 4, 4, 4, 4);
  804.  
  805.   tmpMat[3] = -eyeRight[0];
  806.   tmpMat[7] = -eyeRight[1];
  807.   tmpMat[11] = -eyeRight[2];
  808.   tmpMat[15] = 1;
  809.   multiplication(rollMat, tmpMat, worldMat2, 4, 4, 4, 4);
  810.   /*for (i = 0; i < 16; ++i)
  811.     worldMat[i] = tmpMat[i];*/
  812.  
  813.   //transform tmpMat en camMat
  814.   for (i = 0; i < 4; ++i)
  815.     for (j = 0; j < 4; ++j)
  816.       if (i == j) tmpMat[i * 4 + j] = 1;
  817.       else tmpMat[i * 4 + j] = 0;
  818.  
  819.   /*tmpMat[3] = 0;
  820.     tmpMat[7] = 0;
  821.     tmpMat[11] = 0;
  822.     tmpMat[10] = -1;*/
  823.  
  824.   timeOffset = (millis() - startOffset) / 100;
  825.  
  826.   for (i = 0; i < 10; ++i) {
  827.     for (j = 0; j < 20; ++j) {
  828.       Plocal[0] = -5.0 + i;
  829.       Plocal[1] = 1;
  830.       Plocal[2] = 10.0 - j;// - timeOffset;
  831.       Plocal[3] = 1.0;
  832.  
  833.       if (computePix()) {
  834.         display.drawPixel(Ppix[0], Ppix[1], WHITE);
  835.         if (computePix2()) {
  836.           display2.drawPixel(Ppix[0], Ppix[1], WHITE);
  837.         }
  838.       }
  839.     }
  840.   }
  841.  
  842.   /* draw Objects */
  843.   //I can't have 200 vertices in SRAM because it's like 1kB
  844.   //so I need to load them one by one and compute
  845.   //I could have loaded them on external 23k256
  846.   //or read them from flash each time
  847.   //what is flash read time?
  848.  
  849.   /*
  850.     for (i = 0; i < 5; ++i) {
  851.       Plocal[0] = 0;
  852.       Plocal[1] = - i;
  853.       Plocal[2] = 0;
  854.       Plocal[3] = 1.0;
  855.  
  856.       if (computePix()) {
  857.         display.drawPixel(Ppix[0], Ppix[1], WHITE);
  858.         if (computePix2()) {
  859.           display2.drawPixel(Ppix[0], Ppix[1], WHITE);
  860.         }
  861.       }
  862.     }*/
  863.  
  864.  
  865.  
  866.  
  867.   /*display.println(eyeSocket[0]);
  868.     display.println(eyeSocket[1]);
  869.     display.println(eyeSocket[2]);*/
  870.   if (showDebug && !showKeyb) {
  871.     display.setCursor(0, 0);
  872.     display.println(eyeDirection[0]);
  873.     display.println(eyeDirection[1]);
  874.     display.println(eyeDirection[2]);
  875.     display.println(eyeUp[0]);
  876.     display.println(eyeUp[1]);
  877.     display.println(eyeUp[2]);
  878.   }
  879. }
  880.  
  881.  
  882. bool computePix() {
  883.   int i, j;
  884.  
  885.   multiplication(worldMat, Plocal, Pworld, 4, 4, 4, 1);
  886.   for (i = 0; i < 4; ++i) {
  887.     if (!didOnce) {
  888.       Serial.println(Pworld[i]);
  889.     }
  890.   }
  891.  
  892.   multiplication(tmpMat, Pworld, Pcamera, 4, 4, 4, 1);
  893.   for (i = 0; i < 4; ++i) {
  894.     if (!didOnce) {
  895.       Serial.println(Pcamera[i]);
  896.     }
  897.   }
  898.  
  899.   /*multiplication(rollMat, Pcamera, Pworld, 4, 4, 4, 1);
  900.     for (i = 0; i < 4; ++i)
  901.     Pcamera[i] = Pworld[i];*/
  902.  
  903.   didOnce = true;
  904.  
  905.   if (Pcamera[2] < 1) {
  906.     return false;
  907.   }
  908.  
  909.   float negInvPcameraZ = -(1.0 / (Pcamera[2]));
  910.   for (i = 0; i < 4; ++i)
  911.     Pscreen[i] = Pcamera[i] * negInvPcameraZ;
  912.  
  913.   Pnds[0] = (Pscreen[0] + clipWidth / 2.0) / clipWidth;
  914.   Pnds[1] = (Pscreen[1] + clipHeight / 2.0) / clipHeight;
  915.  
  916.   if (Pnds[0] < 0 || Pnds[0] > 1) return false;
  917.   if (Pnds[1] < 0 || Pnds[1] > 1) return false;
  918.  
  919.   Ppix[0] = (int) (Pnds[0] * 128); //todo dynamic pixelWidth //replaced 128 and 64
  920.   Ppix[1] = (int) ((1 - Pnds[1]) * 64);
  921.   didOnce = true;
  922.   return true;
  923. }
  924.  
  925. bool computePix2() {
  926.   int i, j;
  927.  
  928.   multiplication(worldMat2, Plocal, Pworld, 4, 4, 4, 1);
  929.   for (i = 0; i < 4; ++i) {
  930.     if (!didOnce) {
  931.       Serial.println(Pworld[i]);
  932.     }
  933.   }
  934.  
  935.   multiplication(tmpMat, Pworld, Pcamera, 4, 4, 4, 1);
  936.   for (i = 0; i < 4; ++i) {
  937.     if (!didOnce) {
  938.       Serial.println(Pcamera[i]);
  939.     }
  940.   }
  941.  
  942.   /*multiplication(rollMat, Pcamera, Pworld, 4, 4, 4, 1);
  943.     for (i = 0; i < 4; ++i)
  944.     Pcamera[i] = Pworld[i];*/
  945.  
  946.   didOnce = true;
  947.  
  948.   if (Pcamera[2] < 1) {
  949.     return false;
  950.   }
  951.  
  952.   float negInvPcameraZ = -(1.0 / (Pcamera[2]));
  953.   for (i = 0; i < 4; ++i)
  954.     Pscreen[i] = Pcamera[i] * negInvPcameraZ;
  955.  
  956.   Pnds[0] = (Pscreen[0] + clipWidth / 2.0) / clipWidth;
  957.   Pnds[1] = (Pscreen[1] + clipHeight / 2.0) / clipHeight;
  958.  
  959.   if (Pnds[0] < 0 || Pnds[0] > 1) return false;
  960.   if (Pnds[1] < 0 || Pnds[1] > 1) return false;
  961.  
  962.   Ppix[0] = (int) (Pnds[0] * 128); //todo dynamic pixelWidth //replaced 128 and 64
  963.   Ppix[1] = (int) ((1 - Pnds[1]) * 64);
  964.   didOnce = true;
  965.   return true;
  966. }
  967.  
  968. //For LeftHanded system camTar - camPos
  969. ////For RightHanded system camPos - camTar
  970. void createLookAt(float cameraPosition[], float cameraTarget[], float cameraUp[], float lookAt[]) {
  971.   int i;
  972.  
  973.   //zaxis = normal(cameraTarget - cameraPosition)
  974.   float zaxis[3];
  975.   zaxis[0] = cameraTarget[0] - cameraPosition[0];
  976.   zaxis[1] = cameraTarget[1] - cameraPosition[1];
  977.   zaxis[2] = cameraTarget[2] - cameraPosition[2];
  978.   normalize3(zaxis);
  979.  
  980.   //xaxis = normal(cross(cameraUpVector, zaxis))
  981.   float xaxis[3];
  982.   Vector_Cross_Product(xaxis, cameraUp, zaxis);
  983.   normalize3(xaxis);
  984.  
  985.   //yaxis = cross(zaxis, xaxis)
  986.   float yaxis[3];
  987.   Vector_Cross_Product(yaxis, zaxis, xaxis);
  988.  
  989.   lookAt[0] = xaxis[0];
  990.   lookAt[4] = xaxis[1];
  991.   lookAt[8] = xaxis[2];
  992.  
  993.   lookAt[1] = yaxis[0];
  994.   lookAt[5] = yaxis[1];
  995.   lookAt[9] = yaxis[2];
  996.  
  997.   lookAt[2] = zaxis[0];
  998.   lookAt[6] = zaxis[1];
  999.   lookAt[10] = zaxis[2];
  1000.  
  1001.   lookAt[3] = lookAt[7] = lookAt[11] = 0;
  1002.  
  1003.   lookAt[12] = -Vector_Dot_Product(xaxis, cameraPosition);
  1004.   lookAt[13] = -Vector_Dot_Product(yaxis, cameraPosition);
  1005.   lookAt[14] = -Vector_Dot_Product(zaxis, cameraPosition);
  1006.  
  1007.   lookAt[15] = 1;
  1008.   /*
  1009.     xaxis.x           yaxis.x           zaxis.x          0
  1010.     xaxis.y           yaxis.y           zaxis.y          0
  1011.     xaxis.z           yaxis.z           zaxis.z          0
  1012.     -dot(xaxis, cameraPosition)  -dot(yaxis, cameraPosition)  -dot(zaxis, cameraPosition)  1*/
  1013. }
  1014.  
  1015. void normalize3(float vec[]) {
  1016.   int i;
  1017.   float tmpnormal = sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
  1018.   for (i = 0; i < 3; ++i)
  1019.     vec[i] /= tmpnormal;
  1020. }
  1021. void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
  1022. {
  1023.   vectorOut[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
  1024.   vectorOut[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
  1025.   vectorOut[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
  1026. }
  1027. float Vector_Dot_Product(float vector1[], float vector2[])
  1028. {
  1029.   float op = 0;
  1030.  
  1031.   for (int c = 0; c < 3; c++)
  1032.   {
  1033.     op += vector1[c] * vector2[c];
  1034.   }
  1035.  
  1036.   return op;
  1037. }
  1038. void matrixScalarMult(float m[], int sz, float f) {
  1039.   int i;
  1040.   for (i = 0; i < sz; ++i)
  1041.     m[i] = f * m[i];
  1042. }
  1043. void transformFromAxisAngle(float vecOut[], float v[], float a) {
  1044.   float axis[3];
  1045.   axis[0] = v[0]; axis[1] = v[1]; axis[2] = v[2];
  1046.   float angle = a;
  1047.   normalize3(axis);
  1048.   float s = sin(angle);
  1049.   float c = cos(angle);
  1050.   float oc = 1.0 - c;
  1051.  
  1052.   vecOut[0] = oc * axis[0] * axis[0] + c;
  1053.   vecOut[1] = oc * axis[0] * axis[1] - axis[2] * s;
  1054.   vecOut[2] = oc * axis[2] * axis[0] + axis[1] * s;
  1055.   vecOut[3] = 0.0;
  1056.  
  1057.   vecOut[4] = oc * axis[0] * axis[1] + axis[2] * s;
  1058.   vecOut[5] = oc * axis[1] * axis[1] + c;
  1059.   vecOut[6] = oc * axis[1] * axis[2] - axis[0] * s;
  1060.   vecOut[7] = 0.0;
  1061.  
  1062.   vecOut[8] = oc * axis[2] * axis[0] - axis[1] * s;
  1063.   vecOut[9] = oc * axis[1] * axis[2] + axis[0] * s;
  1064.   vecOut[10] = oc * axis[2] * axis[2] + c;
  1065.   vecOut[11] = 0.0;
  1066.  
  1067.   vecOut[12] = 0.0;
  1068.   vecOut[13] = 0.0;
  1069.   vecOut[14] = 0.0;
  1070.   vecOut[15] = 1.0;
  1071.  
  1072.   /*return mat4(oc * axis.x * axis.x + c,           oc * axis.x * axis.y - axis.z * s,  oc * axis.z * axis.x + axis.y * s,  0.0,
  1073.               oc * axis.x * axis.y + axis.z * s,  oc * axis.y * axis.y + c,           oc * axis.y * axis.z - axis.x * s,  0.0,
  1074.               oc * axis.z * axis.x - axis.y * s,  oc * axis.y * axis.z + axis.x * s,  oc * axis.z * axis.z + c,           0.0,
  1075.               0.0,                                0.0,                                0.0,                                1.0);*/
  1076. }
  1077.  
  1078. void multiplication(float a[], float b[], float mult[], int r1, int c1, int r2, int c2)
  1079. {
  1080.   int i, j, k;
  1081.   for (i = 0; i < r1; ++i)
  1082.     for (j = 0; j < c2; ++j)
  1083.     {
  1084.       mult[i * c2 + j] = 0;
  1085.     }
  1086.  
  1087.   for (i = 0; i < r1; ++i)
  1088.     for (j = 0; j < c2; ++j)
  1089.       for (k = 0; k < c1; ++k)
  1090.       {
  1091.         mult[i * c2 + j] += a[i * c1 + k] * b[k * c2 + j];
  1092.       }
  1093. }
  1094.  
  1095. /************************************************************************************
  1096.    FUNCTIONS IMU
  1097. */
  1098. ////////////////////////////////////////////////////////////
  1099. ///////// IMU functions to get easier all values ///////////
  1100. ////////////////////////////////////////////////////////////
  1101. void MPU9150_setupCompass() {
  1102.   MPU9150_I2C_ADDRESS = 0x0C;      //change Address to Compass
  1103.  
  1104.   MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
  1105.   MPU9150_writeSensor(0x0A, 0x0F); //SelfTest
  1106.   MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
  1107.  
  1108.   MPU9150_I2C_ADDRESS = 0x69;      //change Address to MPU
  1109.  
  1110.   MPU9150_writeSensor(0x24, 0x40); //Wait for Data at Slave0
  1111.   MPU9150_writeSensor(0x25, 0x8C); //Set i2c address at slave0 at 0x0C
  1112.   MPU9150_writeSensor(0x26, 0x02); //Set where reading at slave 0 starts
  1113.   MPU9150_writeSensor(0x27, 0x88); //set offset at start reading and enable
  1114.   MPU9150_writeSensor(0x28, 0x0C); //set i2c address at slv1 at 0x0C
  1115.   MPU9150_writeSensor(0x29, 0x0A); //Set where reading at slave 1 starts
  1116.   MPU9150_writeSensor(0x2A, 0x81); //Enable at set length to 1
  1117.   MPU9150_writeSensor(0x64, 0x01); //overvride register
  1118.   MPU9150_writeSensor(0x67, 0x03); //set delay rate
  1119.   MPU9150_writeSensor(0x01, 0x80);
  1120.  
  1121.   MPU9150_writeSensor(0x34, 0x04); //set i2c slv4 delay
  1122.   MPU9150_writeSensor(0x64, 0x00); //override register
  1123.   MPU9150_writeSensor(0x6A, 0x00); //clear usr setting
  1124.   MPU9150_writeSensor(0x64, 0x01); //override register
  1125.   MPU9150_writeSensor(0x6A, 0x20); //enable master i2c mode
  1126.   MPU9150_writeSensor(0x34, 0x13); //disable slv4
  1127. }
  1128.  
  1129. int MPU9150_readSensor(int addrL, int addrH) {
  1130.   Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  1131.   Wire.write(addrL);
  1132.   Wire.endTransmission(false);
  1133.  
  1134.   Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  1135.   byte L = Wire.read();
  1136.  
  1137.   Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  1138.   Wire.write(addrH);
  1139.   Wire.endTransmission(false);
  1140.  
  1141.   Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  1142.   byte H = Wire.read();
  1143.  
  1144.   return (int16_t)((H << 8) + L);
  1145. }
  1146.  
  1147. int MPU9150_readSensor(int addr) {
  1148.   Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  1149.   Wire.write(addr);
  1150.   Wire.endTransmission(false);
  1151.  
  1152.   Wire.requestFrom(MPU9150_I2C_ADDRESS, 1, true);
  1153.   return Wire.read();
  1154. }
  1155.  
  1156. int MPU9150_writeSensor(int addr, int data) {
  1157.   Wire.beginTransmission(MPU9150_I2C_ADDRESS);
  1158.   Wire.write(addr);
  1159.   Wire.write(data);
  1160.   Wire.endTransmission(true);
  1161.  
  1162.   return 1;
  1163. }
  1164.  
  1165. void setupIMU() {
  1166.   // Initialize the 'Wire' class for the I2C-bus.
  1167.   Wire.begin();
  1168.  
  1169.   // Clear the 'sleep' bit to start the sensor.
  1170.   MPU9150_writeSensor(MPU9150_PWR_MGMT_1, 0);
  1171.  
  1172.   MPU9150_setupCompass();
  1173. }
  1174.  
  1175. void loopIMU() {
  1176.   // Print all sensor values which the sensor provides
  1177.   // Formated all values as x, y, and z in order for
  1178.   // Compass, Gyro, Acceleration. The First value is
  1179.   // the temperature.
  1180.  
  1181.   double dT = ( (double) MPU9150_readSensor(MPU9150_TEMP_OUT_L, MPU9150_TEMP_OUT_H) + 12412.0) / 340.0;
  1182.   Serial.print(dT);
  1183.   Serial.println("C Temperature ");
  1184.   double magX = MPU9150_readSensor(MPU9150_CMPS_XOUT_L, MPU9150_CMPS_XOUT_H);
  1185.   Serial.print(magX);
  1186.   Serial.print("  ");
  1187.   double magY = MPU9150_readSensor(MPU9150_CMPS_YOUT_L, MPU9150_CMPS_YOUT_H);
  1188.   Serial.print(magY);
  1189.   Serial.print("  ");
  1190.   Serial.print(MPU9150_readSensor(MPU9150_CMPS_ZOUT_L, MPU9150_CMPS_ZOUT_H));
  1191.  
  1192.  
  1193.  
  1194.    
  1195.        
  1196.   /*Serial.print("  ");
  1197.   Serial.print(MPU9150_readSensor(MPU9150_GYRO_XOUT_L, MPU9150_GYRO_XOUT_H));
  1198.   Serial.print("  ");
  1199.   Serial.print(MPU9150_readSensor(MPU9150_GYRO_YOUT_L, MPU9150_GYRO_YOUT_H));
  1200.   Serial.print("  ");
  1201.   Serial.print(MPU9150_readSensor(MPU9150_GYRO_ZOUT_L, MPU9150_GYRO_ZOUT_H));*/
  1202.   Serial.println(" x/y/z Compass ");
  1203. /*
  1204.     float magNorthDegrees = 0;
  1205.     if (magY>1) magNorthDegrees = 90 - atan(magX/magY)*(180.0/PI);
  1206.     else if (magY<-1) magNorthDegrees = 270 - atan(magX/magY)*(180.0/PI);
  1207.     else if (magX<-1) magNorthDegrees = 180.0; //Direction (y=0, x<0) = 180.0
  1208.     else if (magX>=-1) magNorthDegrees = 0.0; //Direction (y=0, x>0) = 0.0
  1209.     Serial.print(magNorthDegrees);
  1210.     Serial.println(" Magnetic North");*/
  1211.     double ax, ay, az;
  1212.     ax = MPU9150_readSensor(MPU9150_ACCEL_XOUT_L, MPU9150_ACCEL_XOUT_H);
  1213.   Serial.print(ax);
  1214.   Serial.print("  ");
  1215.   ay = MPU9150_readSensor(MPU9150_ACCEL_YOUT_L, MPU9150_ACCEL_YOUT_H);
  1216.   Serial.print(ay);
  1217.   //Serial.print(MPU9150_readSensor(MPU9150_ACCEL_YOUT_L, MPU9150_ACCEL_YOUT_H));
  1218.   Serial.print("  ");
  1219.   az = MPU9150_readSensor(MPU9150_ACCEL_ZOUT_L, MPU9150_ACCEL_ZOUT_H);
  1220.   Serial.print(az);
  1221.   //Serial.print(MPU9150_readSensor(MPU9150_ACCEL_ZOUT_L, MPU9150_ACCEL_ZOUT_H));
  1222.   Serial.println(" x/y/z Accelero");
  1223.  
  1224.    ///double xyz[3];
  1225.    //double ax, ay, az;
  1226.  
  1227.    //adxl.get_Gxyz(xyz);
  1228.    //ax = xyz[0];
  1229.    //ay = xyz[1];
  1230.    //az = xyz[2];
  1231.    /*double xAngle = atan( ax / (sqrt(square(ay) + square(az))));
  1232.    double yAngle = atan( ay / (sqrt(square(ax) + square(az))));
  1233.    double zAngle = atan( sqrt(square(ax) + square(ay)) / az);
  1234.  
  1235.    xAngle *= 180.00;   yAngle *= 180.00;   zAngle *= 180.00;
  1236.    xAngle /= 3.141592; yAngle /= 3.141592; zAngle /= 3.141592;
  1237.  
  1238.    Serial.print("As angles: "); Serial.print(xAngle); Serial.print("/");
  1239.    Serial.print(yAngle); Serial.print("/"); Serial.println(zAngle); */
  1240.  
  1241.   //delay(10);
  1242. }
  1243.  
  1244.  
  1245. /////////////////////// APPS
  1246.  
  1247. void learnChineseApp() {
  1248.   display.clearDisplay();
  1249.   display2.clearDisplay();
  1250.   //I should drawbitmap logo
  1251.   display.setTextSize(2);
  1252.   display.setTextColor(BLACK, WHITE);
  1253.   display.setCursor(10, 10);
  1254.   display.println("China is Awesome!");
  1255.   display.display();
  1256.   delay(10000);
  1257. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!