Chen Huan
Tsinghua Icenter ChenHuan
Revision 0:9b8df4f9b792, committed 2017-03-16
- Comitter:
- heroistired
- Date:
- Thu Mar 16 13:07:14 2017 +0000
- Commit message:
- DongGanPingTai
Changed in this revision
diff -r 000000000000 -r 9b8df4f9b792 PinMap.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PinMap.h Thu Mar 16 13:07:14 2017 +0000 @@ -0,0 +1,33 @@ +#ifndef __PINMAP_H +#define __PINMAP_H + +//管脚的宏定义 提高用户友好性 +#define Pin_Servo1 PA_8 +#define Pin_Servo2 PA_9 +#define Pin_Servo3 PA_10 +#define Pin_Servo4 PA_11 +#define Pin_Servo5 PA_15 +#define Pin_Servo6 PB_3 +#define Pin_Servo7 PB_10 +#define Pin_Servo8 PB_11 +#define Pin_Servo9 PC_6 +#define Pin_Servo10 PC_7 +#define Pin_Servo11 PC_8 +#define Pin_Servo12 PC_9 +#define Pin_UserUartTx PC_10 +#define Pin_UserUartRx PC_11 +#define Pin_MPU6050SCL PA_13 +#define Pin_MPU6050SDA PA_14 +#define Pin_LeftJoystickX PA_0 +#define Pin_LeftJoystickY PA_1 +#define Pin_RightJoystickX PC_1 +#define Pin_RightJoystickY PC_0 +#define Pin_TFT_CS PA_4 +#define Pin_TFT_SCLK PA_5 +#define Pin_TFT_MISO PA_6 +#define Pin_TFT_MOSI PA_7 +#define Pin_TFT_LED PB_13 +#define Pin_TFT_DC PB_14 +#define Pin_TFT_RESET PB_15 + +#endif \ No newline at end of file
diff -r 000000000000 -r 9b8df4f9b792 TFT/SimpleFont.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TFT/SimpleFont.cpp Thu Mar 16 13:07:14 2017 +0000
@@ -0,0 +1,110 @@
+/***************************************************************
+功能 : mbed的ILI9340液晶显示芯片的驱动程序与图形库的字体库
+作者 : 陈欢 清华大学电机工程与应用电子技术系
+邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
+声明 :
+本程序移植自 https://developer.mbed.org/users/dextorslabs/code/ILI9340_Driver/
+在原有基础上做了改进与丰富,本程序仅供学习与交流使用,如需他用,须联系作者
+All rights reserved
+2017.1.30
+***************************************************************/
+
+const unsigned char simpleFont[][8] =
+{
+ {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x00,0x5F,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x00,0x07,0x00,0x07,0x00,0x00,0x00},
+ {0x00,0x14,0x7F,0x14,0x7F,0x14,0x00,0x00},
+ {0x00,0x24,0x2A,0x7F,0x2A,0x12,0x00,0x00},
+ {0x00,0x23,0x13,0x08,0x64,0x62,0x00,0x00},
+ {0x00,0x36,0x49,0x55,0x22,0x50,0x00,0x00},
+ {0x00,0x00,0x05,0x03,0x00,0x00,0x00,0x00},
+ {0x00,0x1C,0x22,0x41,0x00,0x00,0x00,0x00},
+ {0x00,0x41,0x22,0x1C,0x00,0x00,0x00,0x00},
+ {0x00,0x08,0x2A,0x1C,0x2A,0x08,0x00,0x00},
+ {0x00,0x08,0x08,0x3E,0x08,0x08,0x00,0x00},
+ {0x00,0xA0,0x60,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x08,0x08,0x08,0x08,0x08,0x00,0x00},
+ {0x00,0x60,0x60,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x20,0x10,0x08,0x04,0x02,0x00,0x00},
+ {0x00,0x3E,0x51,0x49,0x45,0x3E,0x00,0x00},
+ {0x00,0x00,0x42,0x7F,0x40,0x00,0x00,0x00},
+ {0x00,0x62,0x51,0x49,0x49,0x46,0x00,0x00},
+ {0x00,0x22,0x41,0x49,0x49,0x36,0x00,0x00},
+ {0x00,0x18,0x14,0x12,0x7F,0x10,0x00,0x00},
+ {0x00,0x27,0x45,0x45,0x45,0x39,0x00,0x00},
+ {0x00,0x3C,0x4A,0x49,0x49,0x30,0x00,0x00},
+ {0x00,0x01,0x71,0x09,0x05,0x03,0x00,0x00},
+ {0x00,0x36,0x49,0x49,0x49,0x36,0x00,0x00},
+ {0x00,0x06,0x49,0x49,0x29,0x1E,0x00,0x00},
+ {0x00,0x00,0x36,0x36,0x00,0x00,0x00,0x00},
+ {0x00,0x00,0xAC,0x6C,0x00,0x00,0x00,0x00},
+ {0x00,0x08,0x14,0x22,0x41,0x00,0x00,0x00},
+ {0x00,0x14,0x14,0x14,0x14,0x14,0x00,0x00},
+ {0x00,0x41,0x22,0x14,0x08,0x00,0x00,0x00},
+ {0x00,0x02,0x01,0x51,0x09,0x06,0x00,0x00},
+ {0x00,0x32,0x49,0x79,0x41,0x3E,0x00,0x00},
+ {0x00,0x7E,0x09,0x09,0x09,0x7E,0x00,0x00},
+ {0x00,0x7F,0x49,0x49,0x49,0x36,0x00,0x00},
+ {0x00,0x3E,0x41,0x41,0x41,0x22,0x00,0x00},
+ {0x00,0x7F,0x41,0x41,0x22,0x1C,0x00,0x00},
+ {0x00,0x7F,0x49,0x49,0x49,0x41,0x00,0x00},
+ {0x00,0x7F,0x09,0x09,0x09,0x01,0x00,0x00},
+ {0x00,0x3E,0x41,0x41,0x51,0x72,0x00,0x00},
+ {0x00,0x7F,0x08,0x08,0x08,0x7F,0x00,0x00},
+ {0x00,0x41,0x7F,0x41,0x00,0x00,0x00,0x00},
+ {0x00,0x20,0x40,0x41,0x3F,0x01,0x00,0x00},
+ {0x00,0x7F,0x08,0x14,0x22,0x41,0x00,0x00},
+ {0x00,0x7F,0x40,0x40,0x40,0x40,0x00,0x00},
+ {0x00,0x7F,0x02,0x0C,0x02,0x7F,0x00,0x00},
+ {0x00,0x7F,0x04,0x08,0x10,0x7F,0x00,0x00},
+ {0x00,0x3E,0x41,0x41,0x41,0x3E,0x00,0x00},
+ {0x00,0x7F,0x09,0x09,0x09,0x06,0x00,0x00},
+ {0x00,0x3E,0x41,0x51,0x21,0x5E,0x00,0x00},
+ {0x00,0x7F,0x09,0x19,0x29,0x46,0x00,0x00},
+ {0x00,0x26,0x49,0x49,0x49,0x32,0x00,0x00},
+ {0x00,0x01,0x01,0x7F,0x01,0x01,0x00,0x00},
+ {0x00,0x3F,0x40,0x40,0x40,0x3F,0x00,0x00},
+ {0x00,0x1F,0x20,0x40,0x20,0x1F,0x00,0x00},
+ {0x00,0x3F,0x40,0x38,0x40,0x3F,0x00,0x00},
+ {0x00,0x63,0x14,0x08,0x14,0x63,0x00,0x00},
+ {0x00,0x03,0x04,0x78,0x04,0x03,0x00,0x00},
+ {0x00,0x61,0x51,0x49,0x45,0x43,0x00,0x00},
+ {0x00,0x7F,0x41,0x41,0x00,0x00,0x00,0x00},
+ {0x00,0x02,0x04,0x08,0x10,0x20,0x00,0x00},
+ {0x00,0x41,0x41,0x7F,0x00,0x00,0x00,0x00},
+ {0x00,0x04,0x02,0x01,0x02,0x04,0x00,0x00},
+ {0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00},
+ {0x00,0x01,0x02,0x04,0x00,0x00,0x00,0x00},
+ {0x00,0x20,0x54,0x54,0x54,0x78,0x00,0x00},
+ {0x00,0x7F,0x48,0x44,0x44,0x38,0x00,0x00},
+ {0x00,0x38,0x44,0x44,0x28,0x00,0x00,0x00},
+ {0x00,0x38,0x44,0x44,0x48,0x7F,0x00,0x00},
+ {0x00,0x38,0x54,0x54,0x54,0x18,0x00,0x00},
+ {0x00,0x08,0x7E,0x09,0x02,0x00,0x00,0x00},
+ {0x00,0x18,0xA4,0xA4,0xA4,0x7C,0x00,0x00},
+ {0x00,0x7F,0x08,0x04,0x04,0x78,0x00,0x00},
+ {0x00,0x00,0x7D,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x80,0x84,0x7D,0x00,0x00,0x00,0x00},
+ {0x00,0x7F,0x10,0x28,0x44,0x00,0x00,0x00},
+ {0x00,0x41,0x7F,0x40,0x00,0x00,0x00,0x00},
+ {0x00,0x7C,0x04,0x18,0x04,0x78,0x00,0x00},
+ {0x00,0x7C,0x08,0x04,0x7C,0x00,0x00,0x00},
+ {0x00,0x38,0x44,0x44,0x38,0x00,0x00,0x00},
+ {0x00,0xFC,0x24,0x24,0x18,0x00,0x00,0x00},
+ {0x00,0x18,0x24,0x24,0xFC,0x00,0x00,0x00},
+ {0x00,0x00,0x7C,0x08,0x04,0x00,0x00,0x00},
+ {0x00,0x48,0x54,0x54,0x24,0x00,0x00,0x00},
+ {0x00,0x04,0x7F,0x44,0x00,0x00,0x00,0x00},
+ {0x00,0x3C,0x40,0x40,0x7C,0x00,0x00,0x00},
+ {0x00,0x1C,0x20,0x40,0x20,0x1C,0x00,0x00},
+ {0x00,0x3C,0x40,0x30,0x40,0x3C,0x00,0x00},
+ {0x00,0x44,0x28,0x10,0x28,0x44,0x00,0x00},
+ {0x00,0x1C,0xA0,0xA0,0x7C,0x00,0x00,0x00},
+ {0x00,0x44,0x64,0x54,0x4C,0x44,0x00,0x00},
+ {0x00,0x08,0x36,0x41,0x00,0x00,0x00,0x00},
+ {0x00,0x00,0x7F,0x00,0x00,0x00,0x00,0x00},
+ {0x00,0x41,0x36,0x08,0x00,0x00,0x00,0x00},
+ {0x00,0x02,0x01,0x01,0x02,0x01,0x00,0x00},
+ {0x00,0x02,0x05,0x05,0x02,0x00,0x00,0x00}
+};
diff -r 000000000000 -r 9b8df4f9b792 TFT/TFT_ILI9340.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TFT/TFT_ILI9340.cpp Thu Mar 16 13:07:14 2017 +0000
@@ -0,0 +1,751 @@
+/***************************************************************
+功能 : mbed的ILI9340液晶显示芯片的驱动程序与图形库,使用硬件SPI
+作者 : 陈欢 清华大学电机工程与应用电子技术系
+邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
+声明 :
+本程序移植自 https://developer.mbed.org/users/dextorslabs/code/ILI9340_Driver/
+在原有基础上做了改进与丰富,本程序仅供学习与交流使用,如需他用,须联系作者
+All rights reserved
+2017.1.30
+***************************************************************/
+
+
+#include "mbed.h"
+#include "TFT_ILI9340.h"
+#include "SimpleFont.cpp"
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Constructor, assigns the pins to the SPI object, set orientation, and sets screen dims.
+////////////////////////////////////////////////////////////////////////////////////////////////
+ILI9340_Display::ILI9340_Display(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName rst, PinName dc)
+ : spi(mosi, miso, sclk), cs(cs), rst(rst), dc(dc) {
+ _height = _TFTHEIGHT;
+ _width = _TFTWIDTH;
+ orientation = 0;
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Command writing code
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::WriteCommand(uint8_t command) {
+ dc = 0;
+ cs = 0;
+ spi.write(command);
+ cs = 1;
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Data writing code
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::WriteData(uint8_t data) {
+ cs = 0;
+ dc = 1;
+ spi.write(data);
+ cs = 1;
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Initilise the display
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DispInit(void) {
+ //CtrlOutput();
+
+ rst = 0;
+
+ // Setup the spi for 8 bit data, high steady state clock,
+ // second edge capture, with a 1MHz clock rate
+ //spi.format(8,3);
+ spi.frequency(24000000); // actually seems to work up to about 20Mhz... way better than the 8mhz as std.
+
+ // Toggle rst to reset
+ rst = 1;
+ wait(0.005);
+ rst = 0;
+ wait(0.020);
+ rst = 1;
+ wait(0.150);
+
+ WriteCommand(0xEF);
+ WriteData(0x03);
+ WriteData(0x80);
+ WriteData(0x02);
+
+ WriteCommand(0xCF);
+ WriteData(0x00);
+ WriteData(0xC1);
+ WriteData(0x30);
+
+ WriteCommand(0xED);
+ WriteData(0x64);
+ WriteData(0x03);
+ WriteData(0x12);
+ WriteData(0x81);
+
+ WriteCommand(0xE8);
+ WriteData(0x85);
+ WriteData(0x00);
+ WriteData(0x78);
+
+ WriteCommand(0xCB);
+ WriteData(0x39);
+ WriteData(0x2C);
+ WriteData(0x00);
+ WriteData(0x34);
+ WriteData(0x02);
+
+ WriteCommand(0xF7);
+ WriteData(0x20);
+
+ WriteCommand(0xEA);
+ WriteData(0x00);
+ WriteData(0x00);
+
+ WriteCommand(ILI9340_PWCTR1); //Power control
+ WriteData(0x23); //VRH[5:0]
+
+ WriteCommand(ILI9340_PWCTR2); //Power control
+ WriteData(0x10); //SAP[2:0];BT[3:0]
+
+ WriteCommand(ILI9340_VMCTR1); //VCM control
+ WriteData(0x3e); //
+ WriteData(0x28);
+
+ WriteCommand(ILI9340_VMCTR2); //VCM control2
+ WriteData(0x86); //--
+
+ WriteCommand(ILI9340_MADCTL); // Memory Access Control
+ WriteData(ILI9340_MADCTL_MX | ILI9340_MADCTL_BGR);
+
+ WriteCommand(ILI9340_PIXFMT);
+ WriteData(0x55);
+
+ WriteCommand(ILI9340_FRMCTR1);
+ WriteData(0x00);
+ WriteData(0x18);
+
+ WriteCommand(ILI9340_DFUNCTR); // Display Function Control
+ WriteData(0x08);
+ WriteData(0x82);
+ WriteData(0x27);
+
+ WriteCommand(0xF2); // 3Gamma Function Disable
+ WriteData(0x00);
+
+ WriteCommand(ILI9340_GAMMASET); //Gamma curve selected
+ WriteData(0x01);
+
+ WriteCommand(ILI9340_GMCTRP1); //Set Gamma
+ WriteData(0x0F);
+ WriteData(0x31);
+ WriteData(0x2B);
+ WriteData(0x0C);
+ WriteData(0x0E);
+ WriteData(0x08);
+ WriteData(0x4E);
+ WriteData(0xF1);
+ WriteData(0x37);
+ WriteData(0x07);
+ WriteData(0x10);
+ WriteData(0x03);
+ WriteData(0x0E);
+ WriteData(0x09);
+ WriteData(0x00);
+
+ WriteCommand(ILI9340_GMCTRN1); //Set Gamma
+ WriteData(0x00);
+ WriteData(0x0E);
+ WriteData(0x14);
+ WriteData(0x03);
+ WriteData(0x11);
+ WriteData(0x07);
+ WriteData(0x31);
+ WriteData(0xC1);
+ WriteData(0x48);
+ WriteData(0x08);
+ WriteData(0x0F);
+ WriteData(0x0C);
+ WriteData(0x31);
+ WriteData(0x36);
+ WriteData(0x0F);
+
+ WriteCommand(ILI9340_SLPOUT); //Exit Sleep
+ wait(0.120);
+ WriteCommand(ILI9340_DISPON); //Display on
+
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Sets the rotation of the display
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 设置屏幕的方向
+//参数 : m 方向 以接线的方向为向下 0 向上 1 向右 2 向下 3 向左
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::SetRotation(uint8_t m) {
+
+ WriteCommand(ILI9340_MADCTL);
+ orientation = m % 4; // can't be higher than 3
+
+ switch (orientation) {
+ case 0:
+ WriteData(ILI9340_MADCTL_MX | ILI9340_MADCTL_BGR);
+ _width = _TFTWIDTH;
+ _height = _TFTHEIGHT;
+ break;
+ case 1:
+ WriteData(ILI9340_MADCTL_MV | ILI9340_MADCTL_BGR);
+ _width = _TFTHEIGHT;
+ _height = _TFTWIDTH;
+ break;
+ case 2:
+ WriteData(ILI9340_MADCTL_MY | ILI9340_MADCTL_BGR);
+ _width = _TFTWIDTH;
+ _height = _TFTHEIGHT;
+ break;
+ case 3:
+ WriteData(ILI9340_MADCTL_MV | ILI9340_MADCTL_MY | ILI9340_MADCTL_MX | ILI9340_MADCTL_BGR);
+ _width = _TFTHEIGHT;
+ _height = _TFTWIDTH;
+ break;
+ }
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Invert the colours of the display in hardware
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::InvertDisplay(bool i) {
+ WriteCommand(i ? ILI9340_INVON : ILI9340_INVOFF);
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Set address window for writing data to.
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::SetAddrWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
+
+ WriteCommand(ILI9340_CASET); // Column addr set
+ WriteData(x0 >> 8);
+ WriteData(x0 & 0xFF); // XSTART
+ WriteData(x1 >> 8);
+ WriteData(x1 & 0xFF); // XEND
+
+ WriteCommand(ILI9340_PASET); // Row addr set
+ WriteData(y0>>8);
+ WriteData(y0); // YSTART
+ WriteData(y1>>8);
+ WriteData(y1); // YEND
+
+ WriteCommand(ILI9340_RAMWR); // write to RAM
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// To draw the humble pixel
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawPixel(uint16_t x, uint16_t y, uint16_t colour) {
+ if((x < 1) ||(x >= _width) || (y < 1) || (y >= _height)) return;
+
+ SetAddrWindow(x,y,x+1,y+1);
+
+ dc = 1;
+ cs = 0;
+
+ spi.write(colour >> 8);
+ spi.write(colour);
+
+ cs = 1;
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Fill the screen with a colour
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::FillScreen(uint16_t colour) {
+ SetAddrWindow(0,0,_width,_height);
+
+ dc = 1;
+ cs = 0;
+
+ unsigned int total = _width * _height;
+ unsigned int position = 0;
+
+ while (position < total) {
+ spi.write(colour >> 8);
+ spi.write(colour);
+ position++;
+ }
+ cs = 1;
+ }
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draws a vertical line fast
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t colour) {
+
+ // Rudimentary clipping
+ if((x >= _width) || (y >= _height)) return;
+
+ if((y+h-1) >= _height)
+ h = _height-y;
+
+ SetAddrWindow(x, y, x, y+h-1);
+
+ uint8_t hi = colour >> 8, lo = colour;
+
+ dc = 1;
+ cs = 0;
+
+ while (h--) {
+ spi.write(hi);
+ spi.write(lo);
+ }
+ cs = 1;
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draws a horizontal line fast
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t colour) {
+
+ // Rudimentary clipping
+ if((x >= _width) || (y >= _height)) return;
+ if((x+w-1) >= _height) w = _width-x;
+ SetAddrWindow(x, y, x+w-1, y);
+
+ uint8_t hi = colour >> 8, lo = colour;
+ dc = 1;
+ cs = 0;
+ while (w--) {
+ spi.write(hi);
+ spi.write(lo);
+ }
+ cs = 1;
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draws a filled rectangle
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::FillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t colour) {
+
+ // rudimentary clipping (drawChar w/big text requires this)
+ if((x >= _width) || (y >= _height)) return;
+ if((x + w - 1) >= _width) w = _width - x;
+ if((y + h - 1) >= _height) h = _height - y;
+
+ SetAddrWindow(x, y, x+w-1, y+h-1);
+
+ uint8_t hi = colour >> 8, lo = colour;
+
+ dc = 1;
+ cs = 0;
+
+ for(y=h; y>0; y--) {
+ for(x=w; x>0; x--) {
+ spi.write(hi);
+ spi.write(lo);
+ }
+ }
+ cs = 1;
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draw an unfilled rectangle
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color){
+ DrawFastHLine(x, y, w, color);
+ DrawFastHLine(x, y+h-1, w, color);
+ DrawFastVLine(x, y, h, color);
+ DrawFastVLine(x+w-1, y, h, color);
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// draw an unfilled circle
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t colour){
+ int16_t f = 1 - r;
+ int16_t ddF_x = 1;
+ int16_t ddF_y = -2 * r;
+ int16_t x = 0;
+ int16_t y = r;
+
+ DrawPixel(x0 , y0+r, colour);
+ DrawPixel(x0 , y0-r, colour);
+ DrawPixel(x0+r, y0 , colour);
+ DrawPixel(x0-r, y0 , colour);
+
+ while (x<y) {
+ if (f >= 0) {
+ y--;
+ ddF_y += 2;
+ f += ddF_y;
+ }
+ x++;
+ ddF_x += 2;
+ f += ddF_x;
+
+ DrawPixel(x0 + x, y0 + y, colour);
+ DrawPixel(x0 - x, y0 + y, colour);
+ DrawPixel(x0 + x, y0 - y, colour);
+ DrawPixel(x0 - x, y0 - y, colour);
+ DrawPixel(x0 + y, y0 + x, colour);
+ DrawPixel(x0 - y, y0 + x, colour);
+ DrawPixel(x0 + y, y0 - x, colour);
+ DrawPixel(x0 - y, y0 - x, colour);
+ }
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draw a filled circle
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::FillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t colour) {
+ DrawFastVLine(x0, y0-r, 2*r+1, colour);
+ FillCircleHelper(x0, y0, r, 3, 0, colour);
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// used to draw circles and roundrects!
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::FillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, int16_t delta, uint16_t colour) {
+
+ int16_t f = 1 - r;
+ int16_t ddF_x = 1;
+ int16_t ddF_y = -2 * r;
+ int16_t x = 0;
+ int16_t y = r;
+
+ while (x<y) {
+ if (f >= 0) {
+ y--;
+ ddF_y += 2;
+ f += ddF_y;
+ }
+ x++;
+ ddF_x += 2;
+ f += ddF_x;
+
+ if (cornername & 0x1) {
+ DrawFastVLine(x0+x, y0-y, 2*y+1+delta, colour);
+ DrawFastVLine(x0+y, y0-x, 2*x+1+delta, colour);
+ }
+ if (cornername & 0x2) {
+ DrawFastVLine(x0-x, y0-y, 2*y+1+delta, colour);
+ DrawFastVLine(x0-y, y0-x, 2*x+1+delta, colour);
+ }
+ }
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// used for drawing rounded corner radii
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawCircleHelper( int16_t x0, int16_t y0, int16_t r, uint8_t cornername, uint16_t colour) {
+ int16_t f = 1 - r;
+ int16_t ddF_x = 1;
+ int16_t ddF_y = -2 * r;
+ int16_t x = 0;
+ int16_t y = r;
+
+ while (x<y) {
+ if (f >= 0) {
+ y--;
+ ddF_y += 2;
+ f += ddF_y;
+ }
+ x++;
+ ddF_x += 2;
+ f += ddF_x;
+ if (cornername & 0x4) {
+ DrawPixel(x0 + x, y0 + y, colour);
+ DrawPixel(x0 + y, y0 + x, colour);
+ }
+ if (cornername & 0x2) {
+ DrawPixel(x0 + x, y0 - y, colour);
+ DrawPixel(x0 + y, y0 - x, colour);
+ }
+ if (cornername & 0x8) {
+ DrawPixel(x0 - y, y0 + x, colour);
+ DrawPixel(x0 - x, y0 + y, colour);
+ }
+ if (cornername & 0x1) {
+ DrawPixel(x0 - y, y0 - x, colour);
+ DrawPixel(x0 - x, y0 - y, colour);
+ }
+ }
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// draw a rounded rectangle!
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawRoundRect(int16_t x, int16_t y, int16_t w, int16_t h, int16_t r, uint16_t colour) {
+ // smarter version
+ DrawFastHLine(x+r , y , w-2*r, colour); // Top
+ DrawFastHLine(x+r , y+h-1, w-2*r, colour); // Bottom
+ DrawFastVLine( x , y+r , h-2*r, colour); // Left
+ DrawFastVLine( x+w-1, y+r , h-2*r, colour); // Right
+ // draw four corners
+ DrawCircleHelper(x+r , y+r , r, 1, colour);
+ DrawCircleHelper(x+w-r-1, y+r , r, 2, colour);
+ DrawCircleHelper(x+w-r-1, y+h-r-1, r, 4, colour);
+ DrawCircleHelper(x+r , y+h-r-1, r, 8, colour);
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// fill a rounded rectangle!
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::FillRoundRect(int16_t x, int16_t y, int16_t w, int16_t h, int16_t r, uint16_t colour) {
+ // smarter version
+ FillRect(x+r, y, w-2*r, h, colour);
+
+ // draw four corners
+ FillCircleHelper(x+w-r-1, y+r, r, 1, h-2*r-1, colour);
+ FillCircleHelper(x+r , y+r, r, 2, h-2*r-1, colour);
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Pass 8-bit (each) R,G,B, get back 16-bit packed color
+////////////////////////////////////////////////////////////////////////////////////////////////
+uint16_t ILI9340_Display::Colour565(uint8_t r, uint8_t g, uint8_t b) {
+ return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Writes an ascii character to the display
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawAscii(unsigned char ascii, uint16_t x, uint16_t y, uint16_t size, uint16_t colour) {
+ SetAddrWindow(x, y, x+size, y+size);
+
+ if( (ascii < 0x20) || (ascii > 0x7e) ) //check for valid ASCII char
+ {
+ ascii = '?'; //char not supported
+ }
+ for(unsigned char i=0; i<8; i++)
+ {
+ unsigned char temp = simpleFont[ascii - 0x20][i];
+ for(unsigned char f=0; f<8; f++)
+ {
+ if( (temp>>f) & 0x01 )
+ {
+ switch(orientation)
+ {
+ case '0':
+ FillRect(x+f*size, y-i*size, size, size, colour);
+ break;
+ case '1':
+ FillRect(x-i*size, x-f*size, size, size, colour);
+ break;
+ case '2':
+ FillRect(x-f*size, y+i*size, size, size, colour);
+ break;
+ case '3':
+ default:
+ FillRect(x+i*size, y+f*size, size, size, colour);
+ }
+ }
+ }
+ }
+}
+
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 将一个字符串显示屏幕上
+//参数 : string 要显示的字符串
+// x,y 字符串中第一个字符的左上角像素的坐标值
+// size 字号 可选 1 2 3
+// colour 像素颜色
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawString(char *string, uint16_t x, uint16_t y, uint8_t size, uint16_t colour)
+{
+ while(*string)
+ {
+ DrawAscii(*string, x, y, size, colour);
+ *string++;
+ switch(orientation)
+ {
+ case '0':
+ if(y > 0) y-=8*size; //Change position to next char
+ break;
+ case '1':
+ if(x > 0) x-=8*size;
+ break;
+ case '2':
+ if(y < _height) y+=8*size;
+ break;
+ case '3':
+ default:
+ if(x < _width) x+=8*size;
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 将一个整数数据显示屏幕上
+//参数 : buffer 显示数据的字符串形式(输出量)
+// value 要显示的整数
+// spaceonbuffer 对整数的位数限制
+// countbase 计数进制
+// x,y 最高位数字的左上角像素的坐标值 如spaceonbuffer为5,则是万位数字的相应坐标,不论实际上该位有无数字
+// size 字号 可选 1 2 3
+// colour 像素颜色
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::IntToChars (char* buffer, int value, uint8_t spaceonbuffer, uint8_t countbase, uint16_t x, uint16_t y, uint8_t size, uint16_t colour) {
+ int workvalue = value;
+ int i;
+ int valuetowrite;
+ int end_i = 0;
+
+ if (value < 0)
+ {
+ workvalue = -value;
+ end_i = 1;
+ buffer[0] = '-';
+ }
+
+ for (i = spaceonbuffer - 1; i >= end_i; i--)
+ {
+ valuetowrite = (workvalue % countbase);
+ if (workvalue == 0)
+ {
+ if (i == (spaceonbuffer - 1))
+ {
+ buffer[i] = 48; // ASCII 0
+ } else {
+ buffer[i] = 32; // ASCII SPACE
+ }
+ } else {
+ if (valuetowrite > 9)
+ {
+ buffer[i] = valuetowrite + 55; // ASCII A-Z
+ } else {
+ buffer[i] = valuetowrite + 48; // ASCII of that character
+ }
+ };
+ workvalue = (workvalue - valuetowrite) / countbase;
+ }
+
+ DrawString(buffer, x, y, size, colour);
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Functional code to swap data contents of 16bit registers
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::Swap(int16_t *a, int16_t *b) {
+
+ int16_t x = *a;
+ *a = *b;
+ *b = x;
+ }
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Draws a line with any length and orientation
+////////////////////////////////////////////////////////////////////////////////////////////////
+void ILI9340_Display::DrawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t colour){
+ int16_t steep = abs(y1 - y0) > abs(x1 - x0);
+ if (steep) {
+ Swap(&x0, &y0);
+ Swap(&x1, &y1);
+ }
+
+ if (x0 > x1) {
+ Swap(&x0, &x1);
+ Swap(&y0, &y1);
+ }
+
+ int16_t dx, dy;
+ dx = x1 - x0;
+ dy = abs(y1 - y0);
+
+ int16_t err = dx / 2;
+ int16_t ystep;
+
+ if (y0 < y1) {
+ ystep = 1;
+ } else {
+ ystep = -1;
+ }
+
+ for (; x0<=x1; x0++) {
+ if (steep) {
+ DrawPixel(y0, x0, colour);
+ } else {
+ DrawPixel(x0, y0, colour);
+ }
+ err -= dy;
+ if (err < 0) {
+ y0 += ystep;
+ err += dx;
+ }
+ }
+}
+
diff -r 000000000000 -r 9b8df4f9b792 TFT/TFT_ILI9340.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TFT/TFT_ILI9340.h Thu Mar 16 13:07:14 2017 +0000
@@ -0,0 +1,145 @@
+#ifndef __TFT_ILI9340_H
+#define __TFT_ILI9340_H
+/***************************************************************
+功能 : mbed的ILI9340液晶显示芯片的驱动程序与图形库,使用硬件SPI
+作者 : 陈欢 清华大学电机工程与应用电子技术系
+邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
+声明 :
+本程序移植自 https://developer.mbed.org/users/dextorslabs/code/ILI9340_Driver/
+在原有基础上做了改进与丰富,本程序仅供学习与交流使用,如需他用,须联系作者
+All rights reserved
+2017.1.30
+***************************************************************/
+
+#include "mbed.h"
+#include "PinMap.h"
+
+#define _TFTWIDTH 240
+#define _TFTHEIGHT 320
+
+#define ILI9340_NOP 0x00
+#define ILI9340_SWRESET 0x01
+#define ILI9340_RDDID 0x04
+#define ILI9340_RDDST 0x09
+
+#define ILI9340_SLPIN 0x10
+#define ILI9340_SLPOUT 0x11
+#define ILI9340_PTLON 0x12
+#define ILI9340_NORON 0x13
+
+#define ILI9340_RDMODE 0x0A
+#define ILI9340_RDMADCTL 0x0B
+#define ILI9340_RDPIXFMT 0x0C
+#define ILI9340_RDIMGFMT 0x0A
+#define ILI9340_RDSELFDIAG 0x0F
+
+#define ILI9340_INVOFF 0x20
+#define ILI9340_INVON 0x21
+#define ILI9340_GAMMASET 0x26
+#define ILI9340_DISPOFF 0x28
+#define ILI9340_DISPON 0x29
+
+#define ILI9340_CASET 0x2A
+#define ILI9340_PASET 0x2B
+#define ILI9340_RAMWR 0x2C
+#define ILI9340_RAMRD 0x2E
+
+#define ILI9340_PTLAR 0x30
+#define ILI9340_MADCTL 0x36
+
+
+#define ILI9340_MADCTL_MY 0x80
+#define ILI9340_MADCTL_MX 0x40
+#define ILI9340_MADCTL_MV 0x20
+#define ILI9340_MADCTL_ML 0x10
+#define ILI9340_MADCTL_RGB 0x00
+#define ILI9340_MADCTL_BGR 0x08
+#define ILI9340_MADCTL_MH 0x04
+
+#define ILI9340_PIXFMT 0x3A
+
+#define ILI9340_FRMCTR1 0xB1
+#define ILI9340_FRMCTR2 0xB2
+#define ILI9340_FRMCTR3 0xB3
+#define ILI9340_INVCTR 0xB4
+#define ILI9340_DFUNCTR 0xB6
+
+#define ILI9340_PWCTR1 0xC0
+#define ILI9340_PWCTR2 0xC1
+#define ILI9340_PWCTR3 0xC2
+#define ILI9340_PWCTR4 0xC3
+#define ILI9340_PWCTR5 0xC4
+#define ILI9340_VMCTR1 0xC5
+#define ILI9340_VMCTR2 0xC7
+
+#define ILI9340_RDID1 0xDA
+#define ILI9340_RDID2 0xDB
+#define ILI9340_RDID3 0xDC
+#define ILI9340_RDID4 0xDD
+
+#define ILI9340_GMCTRP1 0xE0
+#define ILI9340_GMCTRN1 0xE1
+/*
+#define ILI9340_PWCTR6 0xFC
+
+*/
+
+// Color definitions
+#define ILI9340_BLACK 0x0000
+#define ILI9340_BLUE 0x001F
+#define ILI9340_RED 0xF800
+#define ILI9340_GREEN 0x07E0
+#define ILI9340_CYAN 0x07FF
+#define ILI9340_MAGENTA 0xF81F
+#define ILI9340_YELLOW 0xFFE0
+#define ILI9340_WHITE 0xFFFF
+
+
+
+class ILI9340_Display {
+
+ public:
+
+ uint16_t _height;
+ uint16_t _width;
+
+ ILI9340_Display(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName rst, PinName dc);
+
+ void DispInit();
+ void WriteCommand(uint8_t);
+ void WriteData(uint8_t);
+ void SetRotation(uint8_t);
+ void InvertDisplay(bool);
+ void SetAddrWindow(uint16_t, uint16_t, uint16_t, uint16_t);
+
+ void DrawPixel(uint16_t, uint16_t, uint16_t);
+ void FillScreen(uint16_t);
+ void DrawFastVLine(int16_t, int16_t, int16_t, uint16_t);
+ void DrawFastHLine(int16_t, int16_t, int16_t, uint16_t);
+ void FillRect(int16_t, int16_t, int16_t, int16_t, uint16_t);
+ void DrawRect(int16_t, int16_t, int16_t, int16_t, uint16_t);
+ void DrawCircle(int16_t, int16_t, int16_t, uint16_t);
+ void FillCircle(int16_t, int16_t, int16_t, uint16_t);
+ void FillCircleHelper(int16_t, int16_t, int16_t, uint8_t, int16_t, uint16_t);
+ void DrawCircleHelper( int16_t, int16_t, int16_t, uint8_t, uint16_t);
+ void DrawRoundRect(int16_t, int16_t, int16_t, int16_t, int16_t, uint16_t);
+ void FillRoundRect(int16_t, int16_t, int16_t, int16_t, int16_t, uint16_t);
+ uint16_t Colour565(uint8_t, uint8_t, uint8_t);
+
+ void DrawAscii(unsigned char, uint16_t, uint16_t, uint16_t, uint16_t);
+ void DrawString(char *string, uint16_t, uint16_t, uint8_t, uint16_t);
+ void IntToChars (char*, int, uint8_t, uint8_t, uint16_t, uint16_t, uint8_t, uint16_t);
+
+ void Swap(int16_t*, int16_t*);
+ void DrawLine(int16_t, int16_t, int16_t, int16_t, uint16_t);
+
+ protected:
+ SPI spi; // mosi, miso, sclk
+ DigitalOut cs;
+ DigitalOut rst;
+ DigitalOut dc;
+
+ uint8_t orientation;
+
+ };
+#endif
\ No newline at end of file
diff -r 000000000000 -r 9b8df4f9b792 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 16 13:07:14 2017 +0000
@@ -0,0 +1,768 @@
+/***************************************************************
+功能 : 基于mbed的动感平台SDK 包含摇杆 位置传感器 舵机 以及液晶屏等外设
+作者 : 陈欢 清华大学电机工程与应用电子技术系
+邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
+声明 :
+本程序仅供学习与交流使用,如需他用,须联系作者
+All rights reserved
+2017.2.15
+***************************************************************/
+
+#include "mbed.h"
+#include "mpu6050_config.h"
+#include "Servo.h"
+#include "TFT_ILI9340.h"
+#include "PinMap.h"
+Serial SystemUart(SERIAL_TX, SERIAL_RX); //系统串口初始化
+
+//////////////////////////////////////////////////////////////////////////////////
+//头文件部分
+//////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////
+//系统相关
+///////////////////////////////////////
+
+//系统监控
+//接受外部调用的变量
+int TimeNow = 0; //当前的系统时间
+float CpuUsage = 0.0; //当前的CPU使用率 以主中断为基准
+unsigned int MainInterruptCounter = 0; //存储系统主中断的总执行次数
+float MainInterruptFreq = 0.0; //存储系统主中断的实际频率 Hz
+//中间变量 不接受外部调用
+int FreqTime = 0; //计算系统中断的实际频率 存储的是测量开始时的系统时间
+int FreqCounter = 0; //计算系统中断的实际频率 存储的是测量开始时的中断执行次数
+bool Flag_MeasureFreq = true; //标志 是否要进行一次频率测量
+
+
+///////////////////////////////////////
+//mpuiic.h
+///////////////////////////////////////
+
+//声明IIC需要用到的IO口
+DigitalOut MPU_IIC_SCL(Pin_MPU6050SCL); //SCL
+DigitalInOut MPU_IIC_SDA(Pin_MPU6050SDA); //SDA
+
+//弥补屏幕LED画错的bug
+DigitalOut TFT_LED(Pin_TFT_LED);
+
+
+//定义IO操作的宏
+#define MPU_SDA_IN() MPU_IIC_SDA.input()
+#define MPU_SDA_OUT() MPU_IIC_SDA.output()
+#define MPU_READ_SDA MPU_IIC_SDA.read() //读取SDA
+
+//兼容性宏定义
+#define delay_us wait_us
+#define delay_ms wait_ms
+#define u8 unsigned char
+#define u16 unsigned short
+
+//IIC所有操作函数
+void MPU_IIC_Delay(void); //MPU IIC延时函数
+void MPU_IIC_Init(void); //初始化IIC的IO口
+void MPU_IIC_Start(void); //发送IIC开始信号
+void MPU_IIC_Stop(void); //发送IIC停止信号
+void MPU_IIC_Send_Byte(u8 txd); //IIC发送一个字节
+u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC读取一个字节
+u8 MPU_IIC_Wait_Ack(void); //IIC等待ACK信号
+void MPU_IIC_Ack(void); //IIC发送ACK信号
+void MPU_IIC_NAck(void); //IIC不发送ACK信号
+
+void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
+u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr);
+
+///////////////////////////////////////
+//mp6050.h
+///////////////////////////////////////
+u8 MPU_Init(void); //初始化MPU6050
+u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
+u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
+u8 MPU_Write_Byte(u8 reg,u8 data); //IIC写一个字节
+u8 MPU_Read_Byte(u8 reg); //IIC读一个字节
+
+u8 MPU_Set_Gyro_Fsr(u8 fsr);
+u8 MPU_Set_Accel_Fsr(u8 fsr);
+u8 MPU_Set_LPF(u16 lpf);
+u8 MPU_Set_Rate(u16 rate);
+u8 MPU_Set_Fifo(u8 sens);
+
+
+short MPU_Get_Temperature(void);
+u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
+u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
+
+///////////////////////////////////////
+//软件方法解算姿态
+///////////////////////////////////////
+Timer SystemTimer; //系统计时器
+Ticker MainInterrupt; //主中断 100Hz
+//加速度 角速度 温度(原始值)
+short aacx = 0, aacy = 0, aacz = 0, gyrox = 0, gyroy = 0, gyroz = 0, temp = 0;
+float Pitch = 0, Yaw = 0, Roll = 0; //绕x,y,z轴的转角
+float ThetaXz = 0, ThetaYz = 0; //重力加速度在XOZ、YOZ面上与z轴夹角
+
+//卡尔曼滤波器相关参量
+#define PI 3.1415926
+struct KalmanStruct
+{
+ float K1;
+ float angle, angle_dot;
+ float Q_angle;// 过程噪声的协方差
+ float Q_gyro;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
+ float R_angle;// 测量噪声的协方差 既测量偏差
+ float dt;//
+ char C_0;
+ float Q_bias, Angle_err;
+ float PCt_0, PCt_1, E;
+ float K_0, K_1, t_0, t_1;
+ float Pdot[4];
+ float PP[2][2];
+};
+
+struct KalmanStruct X_Axis = {0}, Y_Axis = {0};
+
+void MainInterruptIRQ(void); //主中断服务函数
+void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY); //卡尔曼滤波器
+void Processing(void); //用户进程函数
+
+//系统初始化函数
+void System_Init(void);
+
+//////////////////////////////////////////////////////////////////////////////////
+//函数实现部分
+//////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////
+//mpuiic.c
+///////////////////////////////////////
+
+//MPU IIC 延时函数
+void MPU_IIC_Delay(void)
+{
+ delay_us(2);
+}
+//初始化IIC
+void MPU_IIC_Init(void)
+{
+ RCC->APB2ENR|=1<<4; //先使能外设IO PORTC时钟
+ GPIOC->CRH&=0XFFF00FFF; //PC11/12 推挽输出
+ GPIOC->CRH|=0X00033000;
+ GPIOC->ODR|=3<<11; //PC11,12 输出高
+}
+//产生IIC起始信号
+void MPU_IIC_Start(void)
+{
+ MPU_SDA_OUT(); //sda线输出
+ MPU_IIC_SDA=1;
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
+}
+//产生IIC停止信号
+void MPU_IIC_Stop(void)
+{
+ MPU_SDA_OUT();//sda线输出
+ MPU_IIC_SCL=0;
+ MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_SDA=1;//发送I2C总线结束信号
+ MPU_IIC_Delay();
+}
+//等待应答信号到来
+//返回值：1，接收应答失败
+// 0，接收应答成功
+u8 MPU_IIC_Wait_Ack(void)
+{
+ u8 ucErrTime=0;
+ MPU_SDA_IN(); //SDA设置为输入
+ MPU_IIC_SDA=1;MPU_IIC_Delay();
+ MPU_IIC_SCL=1;MPU_IIC_Delay();
+ while(MPU_READ_SDA)
+ {
+ ucErrTime++;
+ if(ucErrTime>250)
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ }
+ MPU_IIC_SCL=0;//时钟输出0
+ return 0;
+}
+//产生ACK应答
+void MPU_IIC_Ack(void)
+{
+ MPU_IIC_SCL=0;
+ MPU_SDA_OUT();
+ MPU_IIC_SDA=0;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+}
+//不产生ACK应答
+void MPU_IIC_NAck(void)
+{
+ MPU_IIC_SCL=0;
+ MPU_SDA_OUT();
+ MPU_IIC_SDA=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+}
+//IIC发送一个字节
+//返回从机有无应答
+//1，有应答
+//0，无应答
+void MPU_IIC_Send_Byte(u8 txd)
+{
+ u8 t;
+ MPU_SDA_OUT();
+ MPU_IIC_SCL=0;//拉低时钟开始数据传输
+ for(t=0;t<8;t++)
+ {
+ MPU_IIC_SDA=(txd&0x80)>>7;
+ txd<<=1;
+ MPU_IIC_SCL=1;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=0;
+ MPU_IIC_Delay();
+ }
+}
+//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
+u8 MPU_IIC_Read_Byte(unsigned char ack)
+{
+ unsigned char i,receive=0;
+ MPU_SDA_IN();//SDA设置为输入
+ for(i=0;i<8;i++ )
+ {
+ MPU_IIC_SCL=0;
+ MPU_IIC_Delay();
+ MPU_IIC_SCL=1;
+ receive<<=1;
+ if(MPU_READ_SDA)receive++;
+ MPU_IIC_Delay();
+ }
+ if (!ack)
+ MPU_IIC_NAck();//发送nACK
+ else
+ MPU_IIC_Ack(); //发送ACK
+ return receive;
+}
+
+///////////////////////////////////////
+//mp6050.c
+///////////////////////////////////////
+//初始化MPU6050
+//返回值:0,成功
+// 其他,错误代码
+u8 MPU_Init(void)
+{
+ u8 res;
+ MPU_IIC_Init();//初始化IIC总线
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
+ delay_ms(100);
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
+ MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
+ MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
+ MPU_Set_Rate(200); //设置采样率200Hz
+ MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
+ MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
+ MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
+ MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
+ res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
+ if(res==MPU_ADDR)//器件ID正确
+ {
+ MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
+ MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
+ MPU_Set_Rate(200); //设置采样率为200Hz
+ }else return 1;
+ return 0;
+}
+//设置MPU6050陀螺仪传感器满量程范围
+//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
+//返回值:0,设置成功
+// 其他,设置失败
+u8 MPU_Set_Gyro_Fsr(u8 fsr)
+{
+ return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
+}
+//设置MPU6050加速度传感器满量程范围
+//fsr:0,±2g;1,±4g;2,±8g;3,±16g
+//返回值:0,设置成功
+// 其他,设置失败
+u8 MPU_Set_Accel_Fsr(u8 fsr)
+{
+ return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
+}
+//设置MPU6050的数字低通滤波器
+//lpf:数字低通滤波频率(Hz)
+//返回值:0,设置成功
+// 其他,设置失败
+u8 MPU_Set_LPF(u16 lpf)
+{
+ u8 data=0;
+ if(lpf>=188)data=1;
+ else if(lpf>=98)data=2;
+ else if(lpf>=42)data=3;
+ else if(lpf>=20)data=4;
+ else if(lpf>=10)data=5;
+ else data=6;
+ return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
+}
+//设置MPU6050的采样率(假定Fs=1KHz)
+//rate:4~1000(Hz)
+//返回值:0,设置成功
+// 其他,设置失败
+u8 MPU_Set_Rate(u16 rate)
+{
+ u8 data;
+ if(rate>1000)rate=1000;
+ if(rate<4)rate=4;
+ data=1000/rate-1;
+ data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
+ return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
+}
+
+//得到温度值
+//返回值:温度值(扩大了100倍)
+short MPU_Get_Temperature(void)
+{
+ u8 buf[2];
+ short raw;
+ float temp;
+ MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
+ raw=((u16)buf[0]<<8)|buf[1];
+ temp=36.53+((double)raw)/340;
+ return temp*100;;
+}
+//得到陀螺仪值(原始值)
+//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
+//返回值:0,成功
+// 其他,错误代码
+u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
+{
+ u8 buf[6],res;
+ res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
+ if(res==0)
+ {
+ *gx=((u16)buf[0]<<8)|buf[1];
+ *gy=((u16)buf[2]<<8)|buf[3];
+ *gz=((u16)buf[4]<<8)|buf[5];
+ }
+ return res;;
+}
+//得到加速度值(原始值)
+//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
+//返回值:0,成功
+// 其他,错误代码
+u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
+{
+ u8 buf[6],res;
+ res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
+ if(res==0)
+ {
+ *ax=((u16)buf[0]<<8)|buf[1];
+ *ay=((u16)buf[2]<<8)|buf[3];
+ *az=((u16)buf[4]<<8)|buf[5];
+ }
+ return res;;
+}
+//IIC连续写
+//addr:器件地址
+//reg:寄存器地址
+//len:写入长度
+//buf:数据区
+//返回值:0,正常
+// 其他,错误代码
+u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
+{
+ u8 i;
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ for(i=0;i<len;i++)
+ {
+ MPU_IIC_Send_Byte(buf[i]); //发送数据
+ if(MPU_IIC_Wait_Ack()) //等待ACK
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ }
+ MPU_IIC_Stop();
+ return 0;
+}
+//IIC连续读
+//addr:器件地址
+//reg:要读取的寄存器地址
+//len:要读取的长度
+//buf:读取到的数据存储区
+//返回值:0,正常
+// 其他,错误代码
+u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
+{
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ while(len)
+ {
+ if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
+ else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
+ len--;
+ buf++;
+ }
+ MPU_IIC_Stop(); //产生一个停止条件
+ return 0;
+}
+//IIC写一个字节
+//reg:寄存器地址
+//data:数据
+//返回值:0,正常
+// 其他,错误代码
+u8 MPU_Write_Byte(u8 reg,u8 data)
+{
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
+ if(MPU_IIC_Wait_Ack()) //等待应答
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Send_Byte(data);//发送数据
+ if(MPU_IIC_Wait_Ack()) //等待ACK
+ {
+ MPU_IIC_Stop();
+ return 1;
+ }
+ MPU_IIC_Stop();
+ return 0;
+}
+//IIC读一个字节
+//reg:寄存器地址
+//返回值:读到的数据
+u8 MPU_Read_Byte(u8 reg)
+{
+ u8 res;
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Send_Byte(reg); //写寄存器地址
+ MPU_IIC_Wait_Ack(); //等待应答
+ MPU_IIC_Start();
+ MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
+ MPU_IIC_Wait_Ack(); //等待应答
+ res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
+ MPU_IIC_Stop(); //产生一个停止条件
+ return res;
+}
+
+///////////////////////////////////////
+//软件方法解算姿态
+///////////////////////////////////////
+
+/**************************************************************************
+函数功能：简易卡尔曼滤波
+入口参数：加速度、角速度
+返回 值：无
+**************************************************************************/
+
+void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY)
+{
+ X_Axis.angle+=(GyroX - X_Axis.Q_bias) * X_Axis.dt; //先验估计
+ X_Axis.Pdot[0]=X_Axis.Q_angle - X_Axis.PP[0][1] - X_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分
+
+ X_Axis.Pdot[1]=-X_Axis.PP[1][1];
+ X_Axis.Pdot[2]=-X_Axis.PP[1][1];
+ X_Axis.Pdot[3]=X_Axis.Q_gyro;
+ X_Axis.PP[0][0] += X_Axis.Pdot[0] * X_Axis.dt; // Pk-先验估计误差协方差微分的积分
+ X_Axis.PP[0][1] += X_Axis.Pdot[1] * X_Axis.dt; // =先验估计误差协方差
+ X_Axis.PP[1][0] += X_Axis.Pdot[2] * X_Axis.dt;
+ X_Axis.PP[1][1] += X_Axis.Pdot[3] * X_Axis.dt;
+
+ X_Axis.Angle_err = AccelX - X_Axis.angle; //zk-先验估计
+
+ X_Axis.PCt_0 = X_Axis.C_0 * X_Axis.PP[0][0];
+ X_Axis.PCt_1 = X_Axis.C_0 * X_Axis.PP[1][0];
+
+ X_Axis.E = X_Axis.R_angle + X_Axis.C_0 * X_Axis.PCt_0;
+
+ X_Axis.K_0 = X_Axis.PCt_0 / X_Axis.E;
+ X_Axis.K_1 = X_Axis.PCt_1 / X_Axis.E;
+
+ X_Axis.t_0 = X_Axis.PCt_0;
+ X_Axis.t_1 = X_Axis.C_0 * X_Axis.PP[0][1];
+
+ X_Axis.PP[0][0] -= X_Axis.K_0 * X_Axis.t_0; //后验估计误差协方差
+ X_Axis.PP[0][1] -= X_Axis.K_0 * X_Axis.t_1;
+ X_Axis.PP[1][0] -= X_Axis.K_1 * X_Axis.t_0;
+ X_Axis.PP[1][1] -= X_Axis.K_1 * X_Axis.t_1;
+
+ X_Axis.angle += X_Axis.K_0 * X_Axis.Angle_err; //后验估计
+ X_Axis.Q_bias += X_Axis.K_1 * X_Axis.Angle_err; //后验估计
+ X_Axis.angle_dot = GyroX - X_Axis.Q_bias; //输出值(后验估计)的微分=角速度
+
+ Y_Axis.angle+=(GyroY - Y_Axis.Q_bias) * Y_Axis.dt; //先验估计
+ Y_Axis.Pdot[0]=Y_Axis.Q_angle - Y_Axis.PP[0][1] - Y_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分
+
+ Y_Axis.Pdot[1]=-Y_Axis.PP[1][1];
+ Y_Axis.Pdot[2]=-Y_Axis.PP[1][1];
+ Y_Axis.Pdot[3]=Y_Axis.Q_gyro;
+ Y_Axis.PP[0][0] += Y_Axis.Pdot[0] * Y_Axis.dt; // Pk-先验估计误差协方差微分的积分
+ Y_Axis.PP[0][1] += Y_Axis.Pdot[1] * Y_Axis.dt; // =先验估计误差协方差
+ Y_Axis.PP[1][0] += Y_Axis.Pdot[2] * Y_Axis.dt;
+ Y_Axis.PP[1][1] += Y_Axis.Pdot[3] * Y_Axis.dt;
+
+ Y_Axis.Angle_err = AccelY - Y_Axis.angle; //zk-先验估计
+
+ Y_Axis.PCt_0 = Y_Axis.C_0 * Y_Axis.PP[0][0];
+ Y_Axis.PCt_1 = Y_Axis.C_0 * Y_Axis.PP[1][0];
+
+ Y_Axis.E = Y_Axis.R_angle + Y_Axis.C_0 * Y_Axis.PCt_0;
+
+ Y_Axis.K_0 = Y_Axis.PCt_0 / Y_Axis.E;
+ Y_Axis.K_1 = Y_Axis.PCt_1 / Y_Axis.E;
+
+ Y_Axis.t_0 = Y_Axis.PCt_0;
+ Y_Axis.t_1 = Y_Axis.C_0 * Y_Axis.PP[0][1];
+
+ Y_Axis.PP[0][0] -= Y_Axis.K_0 * Y_Axis.t_0; //后验估计误差协方差
+ Y_Axis.PP[0][1] -= Y_Axis.K_0 * Y_Axis.t_1;
+ Y_Axis.PP[1][0] -= Y_Axis.K_1 * Y_Axis.t_0;
+ Y_Axis.PP[1][1] -= Y_Axis.K_1 * Y_Axis.t_1;
+
+ Y_Axis.angle += Y_Axis.K_0 * Y_Axis.Angle_err; //后验估计
+ Y_Axis.Q_bias += Y_Axis.K_1 * Y_Axis.Angle_err; //后验估计
+ Y_Axis.angle_dot = GyroY - Y_Axis.Q_bias; //输出值(后验估计)的微分=角速度
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 主中断的服务函数
+//参数 : 无
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+
+void MainInterruptIRQ(void)
+{
+ float cpu_usage = 0;
+ //系统监控部分代码
+ TimeNow = SystemTimer.read_us();
+ if(Flag_MeasureFreq)
+ {
+ FreqTime = TimeNow;
+ FreqCounter = MainInterruptCounter;
+ Flag_MeasureFreq = false;
+ }
+ if((MainInterruptCounter - FreqCounter) == 100)
+ {
+ MainInterruptFreq = 100.0 * 1000000 / (TimeNow - FreqTime);
+ Flag_MeasureFreq = true;
+ }
+ MainInterruptCounter++;
+
+
+ //temp=MPU_Get_Temperature(); //得到温度值
+ MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
+ MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
+ if(gyroy>32768) gyroy-=65536; //数据类型转换 也可通过short强制类型转换
+ if(gyroz>32768) gyroz-=65536; //数据类型转换
+ if(gyrox>32768) gyrox-=65536; //数据类型转换
+ if(aacx>32768) aacx-=65536; //数据类型转换
+ if(aacz>32768) aacz-=65536; //数据类型转换
+ if(aacy>32768) aacy-=65536; //数据类型转换
+
+ ThetaXz=atan2((double)aacy,(double)aacz)*180/PI; //重力加速度在XOZ、YOZ面上与z轴夹角
+ ThetaYz=atan2((double)aacx,(double)aacz)*180/PI; //计算倾角
+ gyroy=gyroy/16.4; //陀螺仪量程转换
+ gyrox=gyrox/16.4; //陀螺仪量程转换
+ Kalman_Filter(ThetaXz,-gyrox, ThetaYz,-gyroy);
+
+ Processing(); //用户进程
+ cpu_usage = (float)(SystemTimer.read_us() - TimeNow) / 10000; //计算CPU使用率
+ if(cpu_usage > 0)
+ CpuUsage = cpu_usage;
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 系统初始化
+//参数 : 无
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+void System_Init(void)
+{
+ SystemTimer.start();
+
+ //TFT.DispInit();
+ //TFT.FillScreen(ILI9340_BLACK);
+ //TFT.SetRotation(3);
+ //TFT.DrawString("System initing......", 50, 120, 3, ILI9340_WHITE);
+
+ MPU_Init();
+ MainInterrupt.attach(&MainInterruptIRQ, 0.01);
+
+ Flag_MeasureFreq = true; //开始一次测量
+
+ X_Axis.K1 = 0.02;
+ X_Axis.Q_angle=0.001;
+ X_Axis.Q_gyro=0.003;
+ X_Axis.R_angle=0.5;
+ X_Axis.dt=0.005;
+ X_Axis.C_0 = 1;
+ X_Axis.Pdot[0] = 0;
+ X_Axis.Pdot[1] = 0;
+ X_Axis.Pdot[2] = 0;
+ X_Axis.Pdot[3] = 0;
+ X_Axis.PP[0][0]=1;
+ X_Axis.PP[0][1]=0;
+ X_Axis.PP[1][0]=1;
+ X_Axis.PP[1][1]=0;
+
+ Y_Axis.K1 = 0.02;
+ Y_Axis.Q_angle=0.001;
+ Y_Axis.Q_gyro=0.003;
+ Y_Axis.R_angle=0.5;
+ Y_Axis.dt=0.005;
+ Y_Axis.C_0 = 1;
+ Y_Axis.Pdot[0] = 0;
+ Y_Axis.Pdot[1] = 0;
+ Y_Axis.Pdot[2] = 0;
+ Y_Axis.Pdot[3] = 0;
+ Y_Axis.PP[0][0]=1;
+ Y_Axis.PP[0][1]=0;
+ Y_Axis.PP[1][0]=1;
+ Y_Axis.PP[1][1]=0;
+}
+
+//此处--推荐--配置外设、定义全局变量
+//-------------------代码块开始
+
+//DigitalOut myled(LED1);
+AnalogIn LeftJoystickX(Pin_LeftJoystickX);
+AnalogIn LeftJoystickY(Pin_LeftJoystickY);
+AnalogIn RightJoystickX(Pin_RightJoystickX);
+AnalogIn RightJoystickY(Pin_RightJoystickY);
+
+float LeftJoystickValueX = 0.0, LeftJoystickValueY = 0.0, RightJoystickValueX = 0.0, RightJoystickValueY = 0.0; //摇杆x,y轴的读数 归一化到 0-1
+
+Servo Servo1(Pin_Servo1);
+Servo Servo2(Pin_Servo2);
+Servo Servo3(Pin_Servo3);
+Servo Servo4(Pin_Servo4);
+Servo Servo5(Pin_Servo5);
+Servo Servo6(Pin_Servo6);
+Servo Servo7(Pin_Servo7);
+Servo Servo8(Pin_Servo8);
+Servo Servo9(Pin_Servo9);
+Servo Servo10(Pin_Servo10);
+Servo Servo11(Pin_Servo11);
+Servo Servo12(Pin_Servo12);
+
+Serial UserUart(PC_10, PC_11);
+
+
+//-------------------代码块结束
+
+ int i = 0, j;
+ float degree1 = 45.0, degree2 = 135.0;
+//////////////////////////////////////////////////////////////////////////////////
+//功能 : 用户进程函数 用于执行用户自定义的后台任务 执行频率是100Hz
+//参数 : 无
+//返回值 : 无
+//////////////////////////////////////////////////////////////////////////////////
+void Processing(void)
+{
+ LeftJoystickValueX = LeftJoystickX.read();
+ LeftJoystickValueY = LeftJoystickY.read();
+ RightJoystickValueX = RightJoystickX.read();
+ RightJoystickValueY = RightJoystickY.read();
+ if((MainInterruptCounter % 50) == 0)
+ {
+ if(i == 0)
+ {
+ i = !i;
+ Servo1.SetDegree(degree1);
+ Servo2.SetDegree(degree1);
+ Servo3.SetDegree(degree1);
+ Servo4.SetDegree(degree1);
+ Servo5.SetDegree(degree1);
+ Servo6.SetDegree(degree1);
+ Servo7.SetDegree(degree1);
+ Servo8.SetDegree(degree1);
+ Servo9.SetDegree(degree1);
+ Servo10.SetDegree(degree1);
+ Servo11.SetDegree(degree1);
+ Servo12.SetDegree(degree1);
+ }
+ else
+ {
+ i = !i;
+ Servo1.SetDegree(degree2);
+ Servo2.SetDegree(degree2);
+ Servo3.SetDegree(degree2);
+ Servo4.SetDegree(degree2);
+ Servo5.SetDegree(degree2);
+ Servo6.SetDegree(degree2);
+ Servo7.SetDegree(degree2);
+ Servo8.SetDegree(degree2);
+ Servo9.SetDegree(degree2);
+ Servo10.SetDegree(degree2);
+ Servo11.SetDegree(degree2);
+ Servo12.SetDegree(degree2);
+ }
+ }
+}
+
+int main()
+{
+ ILI9340_Display TFT = ILI9340_Display(Pin_TFT_MOSI, Pin_TFT_MISO, Pin_TFT_SCLK, Pin_TFT_CS, Pin_TFT_RESET, Pin_TFT_DC); //液晶屏初始化 已知bug 液晶屏类的定义必须在所有DigitalOut类的定义之后否则无法正常工作
+ TFT_LED = 1; //点亮TFT背光
+ TFT.DispInit();
+ TFT.FillScreen(ILI9340_BLACK);
+ TFT.SetRotation(3);
+ TFT.DrawString("iCenter", 76, 86, 3, ILI9340_WHITE);
+ TFT.DrawString("System initing...", 24, 146, 2, ILI9340_WHITE);
+ System_Init();
+ TFT.DrawString("iCenter", 76, 86, 3, ILI9340_BLACK);
+ TFT.DrawString("System initing...", 24, 146, 2, ILI9340_BLACK);
+
+ while(true) {
+ printf("CPU Usage : %.2f\r\n", CpuUsage);
+ printf("Pitch : %.2f\r\n", Y_Axis.angle);
+ printf("Roll : %.2f\r\n", X_Axis.angle);
+ printf("LeftJoystickValueX : %.2f\r\n", LeftJoystickValueX);
+ printf("LeftJoystickValueY : %.2f\r\n", LeftJoystickValueY);
+ printf("RightJoystickValueX : %.2f\r\n", RightJoystickValueX);
+ printf("RightJoystickValueY : %.2f\r\n", RightJoystickValueY);
+
+ // Small amount of text to the display.
+
+
+ // Do the waiting thang...
+ wait(0.5);
+
+ }
+}
diff -r 000000000000 -r 9b8df4f9b792 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Mar 16 13:07:14 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/ad3be0349dc5 \ No newline at end of file
diff -r 000000000000 -r 9b8df4f9b792 mpu6050_config.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpu6050_config.h Thu Mar 16 13:07:14 2017 +0000 @@ -0,0 +1,92 @@ +#ifndef __MPU6050_CONFIG_H +#define __MPU6050_CONFIG_H + +////////////////////////////////////////////////////////////////////////////////// +//本程序只供学习使用，未经作者许可，不得用于其它任何用途 +////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////// +//mp6050.h +/////////////////////////////////////// + +//#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs寄存器,可读取版本号,寄存器手册未提到 +//#define MPU_PROD_ID_REG 0X0C //prod id寄存器,在寄存器手册未提到 +#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X +#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y +#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z +#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A +#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器 +#define MPU_CFG_REG 0X1A //配置寄存器 +#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器 +#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器 +#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器 +#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器 +#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器 +#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器 +#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器 +#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器 +#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器 +#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器 +#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器 +#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器 +#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器 +#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器 +#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器 +#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器 +#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器 +#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器 +#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器 +#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器 +#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器 +#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器 + +#define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器 +#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器 +#define MPU_INT_EN_REG 0X38 //中断使能寄存器 +#define MPU_INT_STA_REG 0X3A //中断状态寄存器 + +#define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器 +#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器 +#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器 +#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器 +#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器 +#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器 + +#define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器 +#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器 + +#define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器 +#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器 +#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器 +#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器 +#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器 +#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器 + +#define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器 +#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器 +#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器 +#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器 + +#define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器 +#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器 +#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器 +#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器 +#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1 +#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2 +#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位 +#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位 +#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器 +#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器 + +//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位). +//如果接V3.3,则IIC地址为0X69(不包含最低位). +#define MPU_ADDR 0X68 + + +////因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2) +//#define MPU_READ 0XD1 +//#define MPU_WRITE 0XD0 + + + +#endif
diff -r 000000000000 -r 9b8df4f9b792 servo/Servo.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/Servo.h Thu Mar 16 13:07:14 2017 +0000
@@ -0,0 +1,33 @@
+#ifndef __SERVO_H
+#define __SERVO_H
+#include "mbed.h"
+
+//舵机类
+/*class Servo
+{
+ public:
+ Servo(PinName pin);
+ void SetDegree(float degree);
+
+ private:
+ PwmOut _pin;
+};*/
+
+class Servo
+{
+ public:
+ Servo(PinName pin) : _pin(pin)
+ {
+ _pin = 0;
+ }
+ void SetDegree(float degree)
+ {
+ float duty = (degree/180.0*2+0.5)/20;
+ _pin.period_ms(20);
+ _pin.write(duty);
+ }
+ private:
+ PwmOut _pin;
+};
+
+#endif
\ No newline at end of file