rajivm1991

GYRO_OLED_PIXEL_CONTROL

Dec 21st, 2020
659
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include<Wire.h>
  2.  
  3. // OLED - SSD1306
  4. int SCL_PIN = A2;
  5. int SDA_PIN = A3;
  6. int RST_PIN = 5;
  7. int DC_PIN = 4;
  8.  
  9. // OLED helper functions
  10. void LEDPIN_Init(void)
  11. {
  12.   pinMode(SCL_PIN, OUTPUT);
  13.   pinMode(SDA_PIN, OUTPUT);
  14.   pinMode(RST_PIN, OUTPUT);
  15.   pinMode(DC_PIN, OUTPUT);
  16. }
  17.  
  18. void LED_DLY_ms(unsigned int ms)
  19. {
  20.   unsigned int a;
  21.   while (ms)
  22.   {
  23.     a = 6675;
  24.     while (a--);
  25.     ms--;
  26.   }
  27.   return;
  28. }
  29.  
  30. void LED_WrCmd(unsigned char cmd)
  31. {
  32.   unsigned char i = 8;
  33.   digitalWrite(DC_PIN, LOW);
  34.   digitalWrite(SCL_PIN, LOW);
  35.   while (i--)
  36.   {
  37.     if (cmd & 0x80)
  38.     {
  39.       digitalWrite(SDA_PIN, HIGH);
  40.     }
  41.     else
  42.     {
  43.       digitalWrite(SDA_PIN, LOW);
  44.     }
  45.     digitalWrite(SCL_PIN, HIGH);
  46.     asm("nop");
  47.     digitalWrite(SCL_PIN, LOW);
  48.     cmd <<= 1;
  49.   }
  50. }
  51.  
  52. void Set_Display_On_Off(unsigned char d)
  53. {
  54.   LED_WrCmd(0xAE | d);
  55. }
  56.  
  57. void Set_Display_Clock(unsigned char d)
  58. {
  59.   LED_WrCmd(0xD5);
  60.   LED_WrCmd(d);
  61. }
  62.  
  63. void Set_Multiplex_Ratio(unsigned char d)
  64. {
  65.   LED_WrCmd(0xA8);
  66.   LED_WrCmd(d);
  67. }
  68.  
  69. void Set_Display_Offset(unsigned char d)
  70. {
  71.   LED_WrCmd(0xD3);
  72.   LED_WrCmd(d);
  73. }
  74.  
  75. void SetStartLine(unsigned char d)
  76. {
  77.   LED_WrCmd(0x40 | d);
  78. }
  79.  
  80. void Set_Charge_Pump(unsigned char d)
  81. {
  82.   LED_WrCmd(0x8D);
  83.   LED_WrCmd(0x10 | d);
  84. }
  85.  
  86. void SetAddressingMode(unsigned char d)
  87. {
  88.   LED_WrCmd(0x20);
  89.   LED_WrCmd(d);
  90. }
  91.  
  92. void Set_Segment_Remap(unsigned char d)
  93. {
  94.   LED_WrCmd(0xA0 | d);
  95. }
  96.  
  97. void Set_Common_Remap(unsigned char d)
  98. {
  99.   LED_WrCmd(0xC0 | d);
  100. }
  101.  
  102. void Set_Common_Config(unsigned char d)
  103. {
  104.   LED_WrCmd(0xDA);
  105.   LED_WrCmd(0x02 | d);
  106. }
  107.  
  108. void SetContrastControl(unsigned char d)
  109. {
  110.   LED_WrCmd(0x81);
  111.   LED_WrCmd(d);
  112. }
  113.  
  114. void Set_Precharge_Period(unsigned char d)
  115. {
  116.   LED_WrCmd(0xD9);
  117.   LED_WrCmd(d);
  118. }
  119.  
  120. void Set_VCOMH(unsigned char d)
  121. {
  122.   LED_WrCmd(0xDB);
  123.   LED_WrCmd(d);
  124. }
  125.  
  126. void Set_Entire_Display(unsigned char d)
  127. {
  128.   LED_WrCmd(0xA4 | d);
  129. }
  130.  
  131. void Set_Inverse_Display(unsigned char d)
  132. {
  133.   LED_WrCmd(0xA6 | d);
  134. }
  135.  
  136. void LED_WrDat(unsigned char data)
  137. {
  138.   unsigned char i = 8;
  139.   digitalWrite(DC_PIN, HIGH);
  140.   digitalWrite(SCL_PIN, LOW);
  141.   while (i--)
  142.   {
  143.     if (data & 0x80)
  144.     {
  145.       digitalWrite(SDA_PIN, HIGH);
  146.     }
  147.     else
  148.     {
  149.       digitalWrite(SDA_PIN, LOW);
  150.     }
  151.     digitalWrite(SCL_PIN, HIGH);
  152.     asm("nop");
  153.     digitalWrite(SCL_PIN, LOW);
  154.     data <<= 1;
  155.   }
  156. }
  157.  
  158. void LED_Fill(unsigned char bmp_data)
  159. {
  160.   unsigned char y, x;
  161.   for (y = 0; y < 8; y++)
  162.   {
  163.     LED_WrCmd(0xb0 + y);
  164.     LED_WrCmd(0x00);
  165.     LED_WrCmd(0x10);
  166.     for (x = 0; x < 128; x++) LED_WrDat(bmp_data);
  167.   }
  168. }
  169.  
  170. void LED_Set_Pos(unsigned char x, unsigned char y)
  171. {
  172.   LED_WrCmd(0xb0 + y);
  173.   LED_WrCmd(((x & 0xf0) >> 4) | 0x10);
  174.   LED_WrCmd((x & 0x0f) | 0x00);
  175. }
  176.  
  177. void LED_Init(void)
  178. {
  179.   digitalWrite(SCL_PIN, HIGH);
  180.   digitalWrite(RST_PIN, LOW);
  181.   LED_DLY_ms(50);
  182.   digitalWrite(RST_PIN, HIGH);
  183.   Set_Display_On_Off(0x00);   // Display Off (0x00/0x01)
  184.   Set_Display_Clock(0x80);    // Set Clock as 100 Frames/Sec
  185.   Set_Multiplex_Ratio(0x3F);  // 1/64 Duty (0x0F~0x3F)
  186.   Set_Display_Offset(0x00);   // Shift Mapping RAM Counter (0x00~0x3F)
  187.   SetStartLine(0x00);         // Set Mapping RAM Display Start Line (0x00~0x3F)
  188.   Set_Charge_Pump(0x04);      // Enable Embedded DC/DC Converter (0x00/0x04)
  189.   SetAddressingMode(0x02);    // Set Page Addressing Mode (0x00/0x01/0x02)
  190.   Set_Segment_Remap(0x01);    // Set SEG/Column Mapping
  191.   Set_Common_Remap(0x08);     // Set COM/Row Scan Direction
  192.   Set_Common_Config(0x10);    // Set Sequential Configuration (0x00/0x10)
  193.   SetContrastControl(0xCF);   // Set SEG Output Current
  194.   Set_Precharge_Period(0xF1); // Set Pre-Charge as 15 Clocks & Discharge as 1 Clock
  195.   Set_VCOMH(0x40);            // Set VCOM Deselect Level
  196.   Set_Entire_Display(0x00);   // Disable Entire Display On (0x00/0x01)
  197.   Set_Inverse_Display(0x00);  // Disable Inverse Display On (0x00/0x01)
  198.   Set_Display_On_Off(0x01);   // Display On (0x00/0x01)
  199.   LED_Fill(0x00);             // clear all
  200.   LED_Set_Pos(0, 0);
  201. }
  202.  
  203. void LED_Plot(int x, int y) {
  204.   if (x > 127) return;
  205.   if (y > 47) return;
  206.   int line_num = y / 8 + 2;
  207.   unsigned char data = 1 << (y % 8);
  208.   LED_Set_Pos(x, line_num);
  209.   LED_WrDat(data);
  210. }
  211.  
  212. void LED_PrintBMP(unsigned char x0, unsigned char y0, unsigned char x1, unsigned char y1, unsigned char bmp[])
  213. {
  214.   int ii = 0;
  215.   unsigned char x, y;
  216.   for (y = y0; y < y1; y++)
  217.   {
  218.     LED_Set_Pos(x0, y);
  219.     for (x = x0; x < x1; x++)
  220.     {
  221.       LED_WrDat(bmp[ii++]);
  222.     }
  223.   }
  224. }
  225.  
  226. // MPU6050
  227.  
  228. const int MPU_addr=0x68;  // I2C address of the MPU-6050
  229. int16_t GyX,GyY,GyZ;
  230.  
  231. const int RunningAverageCount = 17;
  232. int       NextRunningAverageI = 0;
  233. const int Diff = 1;
  234. int       PrevRunningAverageI = RunningAverageCount - Diff;
  235. int       RunningAverageBufferX[RunningAverageCount], RunningAverageBufferY[RunningAverageCount], RunningAverageBufferZ[RunningAverageCount];
  236. float     RunningAverageX = 0, RunningAverageY = 0, RunningAverageZ = 0;
  237.  
  238. const unsigned char BMP1[] = {0x18, 0x18, 0x3C, 0xE7, 0xE7, 0x3C, 0x18, 0x18};
  239. const unsigned char BMP2[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  240. const int BMP_Length = 8, BMP_Height = 1;
  241. const int OriginX = (128 - BMP_Length) / 2;  // graph length = 128 - bmp, so origin is at middle of it
  242. const int OriginY = 8 / 2;                   // 8 lines graph, so start origin at middle
  243. const int OffsetX = 0, OffsetY = 0;
  244. int x1=0, y1=0, x2=BMP_Length, y2=BMP_Height;
  245. const float GyX_Limit = 5000;
  246. const float GyY_Limit = 1000;
  247.  
  248. void limit(int16_t &variable, int value_limit)
  249. {
  250.   variable = (variable > value_limit) ? value_limit : variable;
  251.   variable = (variable < -value_limit) ? -value_limit : variable;
  252. }
  253.  
  254. float scale(int16_t variable, float s, int value_limit)
  255. {
  256.   // x / s = variable / limit;
  257.   float ans = 0;
  258.   ans = s * float(variable) / value_limit;
  259.   ans = (ans == s) ? ans - 1 : ans;
  260.   return ans;
  261. }
  262.  
  263. // Arduino
  264.  
  265. void setup(){  
  266.   // OLED Setup
  267.   LEDPIN_Init();
  268.   LED_Init();
  269.  
  270.   // MPU6050 Setup
  271.   Wire.begin();
  272.   Wire.beginTransmission(MPU_addr);
  273.   Wire.write(0x6B);  // PWR_MGMT_1 register
  274.   Wire.write(0);     // set to zero (wakes up the MPU-6050)
  275.   Wire.endTransmission(true);
  276.   Serial.begin(9600);
  277.  
  278.   for(int i=0; i< RunningAverageCount; ++i) {RunningAverageX = 0; RunningAverageY = 0; RunningAverageZ = 0;}
  279. }
  280.  
  281. void loop(){
  282.   Wire.beginTransmission(MPU_addr);
  283.   Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  284.   Wire.endTransmission(false);
  285.   Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  286.  
  287.   GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  288.   GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  289.   GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  290.  
  291.   limit(GyX, GyX_Limit); limit(GyY, GyY_Limit);
  292.  
  293.   RunningAverageBufferX[NextRunningAverageI] = GyX;
  294.   RunningAverageBufferY[NextRunningAverageI] = GyY;
  295.   RunningAverageBufferZ[NextRunningAverageI] = GyZ;
  296.  
  297.   NextRunningAverageI++;
  298.   NextRunningAverageI = (NextRunningAverageI >= RunningAverageCount) ? 0 : NextRunningAverageI;
  299.   PrevRunningAverageI = (NextRunningAverageI >= Diff) ? NextRunningAverageI - Diff : RunningAverageCount - Diff + NextRunningAverageI;
  300.  
  301.   // clear plot
  302.   LED_PrintBMP(x1, y1, x2, y2, BMP2);
  303.  
  304.   // calculate
  305.   RunningAverageX = 0; RunningAverageY = 0; RunningAverageZ = 0;
  306.   for(int i=0; i< RunningAverageCount; ++i) {
  307.     RunningAverageX += RunningAverageBufferX[i];
  308.     RunningAverageY += RunningAverageBufferY[i];
  309.     RunningAverageZ += RunningAverageBufferZ[i];
  310.   }
  311.   RunningAverageX /= RunningAverageCount;
  312.   RunningAverageY /= RunningAverageCount;
  313.   RunningAverageZ /= RunningAverageCount;
  314.  
  315.   // direct value
  316.   // RunningAverageX = GyX; RunningAverageY = GyY;
  317.  
  318.   // plot
  319.   // LED_Plot(scale(RunningAverageY, OriginX) + OriginX + OffsetX, scale(RunningAverageX, OriginY) + OriginY + OffsetY);
  320.   x1 = 0 + scale(RunningAverageY, OriginX, GyY_Limit) + OriginX + OffsetX;
  321.   y1 = 0 + scale(RunningAverageX, OriginY, GyX_Limit) + OriginY + OffsetY;
  322.   x2 = 8 + scale(RunningAverageY, OriginX, GyY_Limit) + OriginX + OffsetX;
  323.   y2 = 1 + scale(RunningAverageX, OriginY, GyX_Limit) + OriginY + OffsetY;
  324.   LED_PrintBMP(x1, y1, x2, y2, BMP1);
  325.   Serial.println(String(scale(RunningAverageY, OriginX, GyY_Limit)) + " " + String(+scale(RunningAverageX, OriginY, GyX_Limit)));
  326.  
  327.   delay(200);
  328. }
RAW Paste Data