main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS0_SH.h"
00003 
00004 #define pi 3.141592f
00005 #define d2r 0.01745329f
00006 
00007 #define Rms 5000            //TT rate
00008 #define dt  0.005f
00009 #define NN  200
00010 
00011 #define Kp  3.6f
00012 #define Ki  5.0f
00013 #define Kd  0.12f
00014 #define Kcon 0.00f
00015 
00016 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
00017 
00018 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00019 //~~~structure~~~//
00020 DigitalOut  led(D13);           //detection
00021 DigitalOut  TT_ext(D12);
00022 
00023 //~~~IMU_SPI~~~//
00024 DigitalOut  SPI_CSG(D7,1);      //low for GYRO enable
00025 DigitalOut  SPI_CSXM(D6,1);     //low for ACC/MAG enable
00026 SPI spi(D4, D5, D3);            //MOSI MISO SCLK
00027 
00028 //~~~Serial~~~//
00029 Serial      pc(D1, D0);         //Serial reg(TX RX)
00030 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00031 
00032 
00033 
00034 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00035 //~~~globle~~~//
00036 Ticker  TT;                         //call a timer
00037 int     count = 0;                  //one second counter for extrenal led blink
00038 
00039 //~~~IMU_SPI~~~//
00040 short low_byte = 0x00;              //buffer
00041 short high_byte = 0x00;
00042 short Buff = 0x00;
00043 float Wx = 0.0;
00044 float Wy = 0.0;
00045 float Wz = 0.0;
00046 float Ax = 0.0;
00047 float Ay = 0.0;
00048 float Az = 0.0;
00049 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00050 
00051 
00052 
00053 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00054 void    init_TIMER();           //set TT_main() rate
00055 void    TT_main();              //timebase function rated by TT
00056 void    init_IO();              //initialize IO state
00057 
00058 void    init_IMU();             //initialize IMU
00059 void    read_IMU();             //read IMU data give raw data
00060 
00061 float   lpf(float input, float output_old, float frequency);    //lpf discrete
00062 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00063 
00064 
00065 
00066 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00067 int main()
00068 {
00069     pc.baud(115200);            //set baud rate
00070 
00071     init_IO();                  //initialized value
00072     init_IMU();                 //initialize IMU
00073 
00074     init_TIMER();               //start TT_main
00075 
00076     while(1) {                  //main() loop
00077         if(count >= NN) {       //check if main working
00078             count=0;
00079             led = !led;
00080         }
00081     }
00082 
00083 }
00084 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00085 
00086 
00087 
00088 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00089 void init_TIMER()                   //set TT_main{} rate
00090 {
00091     TT.attach_us(&TT_main, Rms);
00092 }
00093 void TT_main()                      //interrupt function by TT
00094 {
00095     TT_ext = !TT_ext;               //indicate TT_main() function working
00096     count = count+1;                //one second counter
00097 
00098     read_IMU();                     //read IMU data give raw data
00099 
00100     //for Serial-Oscilloscope
00101     pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
00102 
00103 }
00104 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00105 
00106 
00107 
00108 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00109 void init_IO(void)                  //initialize
00110 {
00111     TT_ext = 0;
00112     led = 1;
00113 }
00114 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00115 
00116 
00117 
00118 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00119 void init_IMU(void)                 //initialize
00120 {
00121     //gloable config
00122     SPI_CSXM = 1;                   //high as init for disable SPI
00123     SPI_CSG = 1;
00124     spi.format(8, 3);               //byte width, spi mode
00125     spi.frequency(4000000);         //8MHz
00126 
00127     //for GYRO config
00128     SPI_CSG = 0;                    //start spi talking
00129     spi.write(CTRL_REG1_G);
00130     spi.write(0x9F);                //data rate 380 Hz/ cut off 25 Hz
00131     SPI_CSG = 1;                    //end spi talking
00132 
00133     SPI_CSG = 0;                    //start spi talking
00134     spi.write(CTRL_REG4_G);
00135     spi.write(0x10);                //Scle 500dps
00136     SPI_CSG = 1;                    //end spi talking
00137 
00138     //for ACC config
00139     SPI_CSXM = 0;                   //start spi talking
00140     spi.write(CTRL_REG1_XM);
00141     spi.write(0x87);                //data rate 400 Hz/ Enable
00142     SPI_CSXM = 1;                   //end spi talking
00143 
00144     SPI_CSXM = 0;                   //start spi talking
00145     spi.write(CTRL_REG2_XM);
00146     spi.write(0xC8);                //cut off 50 Hz/ Scale +-4g
00147     SPI_CSXM = 1;                   //end spi talking
00148 }
00149 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00150 
00151 
00152 
00153 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00154 void read_IMU(void)                 //read IMU data give raw data
00155 {
00156     //Wx
00157     SPI_CSG = 0;                    //start spi talking Wx
00158     spi.write(0xE8);                //read B11101000 read/multi/address
00159     low_byte = spi.write(0);
00160     high_byte = spi.write(0);
00161     Buff = high_byte << 8 |low_byte;
00162     SPI_CSG = 1;                    //end spi talking
00163 //    Wx = Buff * Gpx + Gdx;
00164     Wx = lpf(Buff * Gpx, Wx, 48.0f);
00165     //Wy
00166     SPI_CSG = 0;                    //start spi talking Wx
00167     spi.write(0xEA);                //read B11101010 read/multi/address
00168     low_byte = spi.write(0);
00169     high_byte = spi.write(0);
00170     Buff = high_byte << 8 |low_byte;
00171     SPI_CSG = 1;                    //end spi talking
00172 //    Wy = Buff * Gpy + Gdy;
00173     Wy = lpf(Buff * Gpy, Wy, 48.0f);
00174     //Wz
00175     SPI_CSG = 0;                    //start spi talking Wx
00176     spi.write(0xEC);                //read B11101100 read/multi/address
00177     low_byte = spi.write(0);
00178     high_byte = spi.write(0);
00179     Buff = high_byte << 8 |low_byte;
00180     SPI_CSG = 1;                    //end spi talking
00181 //    Wz = Buff * Gpz + Gdz;
00182     Wz = lpf(Buff * Gpz, Wz, 48.0f);
00183     //Ax
00184     SPI_CSXM = 0;                   //start spi talking Ax
00185     spi.write(0xE8);                //read B11101000 read/multi/address
00186     low_byte = spi.write(0);
00187     high_byte = spi.write(0);
00188     Buff = high_byte << 8 |low_byte;
00189     SPI_CSXM = 1;                   //end spi talking
00190     Ax = lpf(Buff * Apx, Ax, 13.0f);
00191     //Ay
00192     SPI_CSXM = 0;                   //start spi talking Ax
00193     spi.write(0xEA);                //read B11101010 read/multi/address
00194     low_byte = spi.write(0);
00195     high_byte = spi.write(0);
00196     Buff = high_byte << 8 |low_byte;
00197     SPI_CSXM = 1;                   //end spi talking
00198     Ay = lpf(Buff * Apy, Ay, 13.0f);
00199     //Az
00200     SPI_CSXM = 0;                   //start spi talking Ax
00201     spi.write(0xEC);                //read B11101100 read/multi/address
00202     low_byte = spi.write(0);
00203     high_byte = spi.write(0);
00204     Buff = high_byte << 8 |low_byte;
00205     SPI_CSXM = 1;                   //end spi talking
00206     Az = lpf(Buff * Apz, Az, 13.0f);
00207 }
00208 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
00209 
00210 
00211 
00212 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
00213 float lpf(float input, float output_old, float frequency)
00214 {
00215     float output = 0;
00216     output = (output_old + frequency*dt*input) / (1 + frequency*dt);
00217     return output;
00218 }
00219 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//