main.cpp

00001 /*this program is written for multicoptor project
00002  *Developper :lokitsyu azlab
00003  *mbed.org
00004  *AC-room
00005  *mltcptdrv filename:main.cpp
00006  *using gyro senser ITG-3200
00007  */
00008 
00009 #include "mbed.h"
00010 
00011 InterruptIn unit(p8);
00012 I2C i2c(p28,p27);
00013 Ticker timer;
00014 
00015 
00016 int addr[4]={
00017     0x20, 0x21, 0x22, 0x23
00018 };//motor driver address
00019 
00020 int outval_1, outval_2, outval_3, outval_4;
00021 int outval[4] = {
00022     outval_1, outval_2, outval_3, outval_4
00023 };//output value
00024 
00025 
00026 //////////////////////////////////////////////////////////////
00027 //Gyro class
00028 //////////////////////////////////////////////////////////////
00029 class Gyro{
00030     private:
00031         int ax_1, ax_2, ax_3, 
00032             ay_1, ay_2, ay_3, 
00033             az_1, az_2, az_3;
00034         float ax, ay, az;        //angr value
00035         double sum;
00036     public:
00037         void get();
00038         void set();
00039         void calc();
00040         double simpson(double,double,double);
00041 };
00042 //get gyro value
00043 void Gyro::get()
00044 {
00045     int i = 0;
00046         i2c.start();
00047     if(i2c.write(11010001)==1){
00048         i2c.write();
00049         i2c.start();
00050         ax_1 = i2c.read();
00051         i2c.stop();
00052     }else{
00053         i2c.stop();
00054         i = !i;
00055     }
00056     if(i == 0){
00057         ax_1 = ax_1 << 8;
00058         i2c.start();
00059         i2c.write(11010001);
00060         i2c.write();
00061         i2c.start();
00062         ax_1 = ax_1 + i2c.read();
00063         i2c.stop();
00064     }else{
00065     }
00066 }
00067 
00068 //setting gyro sencer
00069 void Gyro::set()
00070 {
00071 }
00072 
00073 void Gyro::calc()
00074 {
00075 }
00076 
00077 double Gyro::simpson(double f_0,double f_1,double f_2)
00078 {
00079     sum += (f_0 + 4 * f_1 + f_2)/6000;
00080     return sum;
00081 }
00082 
00083 
00084 ///////////////////////////////////////////////////////////////////////
00085 //motor drive control funciton
00086 ///////////////////////////////////////////////////////////////////////
00087 void motordrv()
00088 {
00089     for(int i=0;i<4;i++)
00090     {
00091         i2c.start();
00092         i2c.write(addr[i],outval[i],1);
00093         i2c.stop();
00094     }
00095 }
00096 
00097 ///////////////////////////////////////////////////////////////////////
00098 //main function
00099 ///////////////////////////////////////////////////////////////////////
00100 int main() {
00101     Gyro gyro;
00102     
00103     i2c.frequency(400000);
00104     unit.mode(PullUp);
00105     unit.fall(&gyro,&Gyro::get);
00106     timer.attach_us(&motordrv,10000);
00107     
00108     gyro.set();
00109     
00110     //main loop
00111     while(1) {
00112         gyro.calc();
00113         /*
00114     
00115     
00116          Write Program on This Space
00117         
00118     
00119         */    
00120     }
00121 }