tsyu loki / Mbed 2 deprecated mltcptdrv

test program

Dependencies:   mbed-rtos mbed

main.cpp@2:906c20c9d15e, 2013-07-13 (annotated)

Committer:
lokitsyu
Date:
Sat Jul 13 05:52:47 2013 +0000
Revision:
2:906c20c9d15e
Parent:
1:5c6888eb051a 
not good yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lokitsyu 1:5c6888eb051a 1 /*this program is written for multicoptor project
lokitsyu 1:5c6888eb051a 2 *Developper :lokitsyu azlab
lokitsyu 1:5c6888eb051a 3 *mbed.org
lokitsyu 1:5c6888eb051a 4 *AC-room
lokitsyu 1:5c6888eb051a 5 *mltcptdrv filename:main.cpp
lokitsyu 1:5c6888eb051a 6 *using gyro senser ITG-3200
lokitsyu 1:5c6888eb051a 7 */
lokitsyu 1:5c6888eb051a 8
lokitsyu 0:3137e34fc9fa 9 #include "mbed.h"
lokitsyu 1:5c6888eb051a 10
lokitsyu 1:5c6888eb051a 11 InterruptIn unit(p8);
lokitsyu 1:5c6888eb051a 12 I2C i2c(p28,p27);
lokitsyu 1:5c6888eb051a 13 Ticker timer;
lokitsyu 1:5c6888eb051a 14
lokitsyu 1:5c6888eb051a 15
lokitsyu 1:5c6888eb051a 16 int addr[4]={
lokitsyu 1:5c6888eb051a 17 0x20, 0x21, 0x22, 0x23
lokitsyu 1:5c6888eb051a 18 };//motor driver address
lokitsyu 1:5c6888eb051a 19
lokitsyu 1:5c6888eb051a 20 int outval_1, outval_2, outval_3, outval_4;
lokitsyu 1:5c6888eb051a 21 int outval[4] = {
lokitsyu 1:5c6888eb051a 22 outval_1, outval_2, outval_3, outval_4
lokitsyu 1:5c6888eb051a 23 };//output value
lokitsyu 0:3137e34fc9fa 24
lokitsyu 0:3137e34fc9fa 25
lokitsyu 1:5c6888eb051a 26 //////////////////////////////////////////////////////////////
lokitsyu 1:5c6888eb051a 27 //Gyro class
lokitsyu 1:5c6888eb051a 28 //////////////////////////////////////////////////////////////
lokitsyu 1:5c6888eb051a 29 class Gyro{
lokitsyu 1:5c6888eb051a 30 private:
lokitsyu 1:5c6888eb051a 31 int ax_1, ax_2, ax_3,
lokitsyu 1:5c6888eb051a 32 ay_1, ay_2, ay_3,
lokitsyu 1:5c6888eb051a 33 az_1, az_2, az_3;
lokitsyu 1:5c6888eb051a 34 float ax, ay, az; //angr value
lokitsyu 2:906c20c9d15e 35 double sum;
lokitsyu 1:5c6888eb051a 36 public:
lokitsyu 1:5c6888eb051a 37 void get();
lokitsyu 1:5c6888eb051a 38 void set();
lokitsyu 1:5c6888eb051a 39 void calc();
lokitsyu 2:906c20c9d15e 40 double simpson(double,double,double);
lokitsyu 0:3137e34fc9fa 41 };
lokitsyu 1:5c6888eb051a 42 //get gyro value
lokitsyu 1:5c6888eb051a 43 void Gyro::get()
lokitsyu 1:5c6888eb051a 44 {
lokitsyu 1:5c6888eb051a 45 int i = 0;
lokitsyu 1:5c6888eb051a 46 i2c.start();
lokitsyu 1:5c6888eb051a 47 if(i2c.write(11010001)==1){
lokitsyu 1:5c6888eb051a 48 i2c.write();
lokitsyu 1:5c6888eb051a 49 i2c.start();
lokitsyu 1:5c6888eb051a 50 ax_1 = i2c.read();
lokitsyu 1:5c6888eb051a 51 i2c.stop();
lokitsyu 1:5c6888eb051a 52 }else{
lokitsyu 1:5c6888eb051a 53 i2c.stop();
lokitsyu 1:5c6888eb051a 54 i = !i;
lokitsyu 1:5c6888eb051a 55 }
lokitsyu 1:5c6888eb051a 56 if(i == 0){
lokitsyu 1:5c6888eb051a 57 ax_1 = ax_1 << 8;
lokitsyu 1:5c6888eb051a 58 i2c.start();
lokitsyu 1:5c6888eb051a 59 i2c.write(11010001);
lokitsyu 1:5c6888eb051a 60 i2c.write();
lokitsyu 1:5c6888eb051a 61 i2c.start();
lokitsyu 1:5c6888eb051a 62 ax_1 = ax_1 + i2c.read();
lokitsyu 1:5c6888eb051a 63 i2c.stop();
lokitsyu 2:906c20c9d15e 64 }else{
lokitsyu 2:906c20c9d15e 65 }
lokitsyu 1:5c6888eb051a 66 }
lokitsyu 0:3137e34fc9fa 67
lokitsyu 1:5c6888eb051a 68 //setting gyro sencer
lokitsyu 1:5c6888eb051a 69 void Gyro::set()
lokitsyu 1:5c6888eb051a 70 {
lokitsyu 1:5c6888eb051a 71 }
lokitsyu 0:3137e34fc9fa 72
lokitsyu 1:5c6888eb051a 73 void Gyro::calc()
lokitsyu 1:5c6888eb051a 74 {
lokitsyu 1:5c6888eb051a 75 }
lokitsyu 1:5c6888eb051a 76
lokitsyu 2:906c20c9d15e 77 double Gyro::simpson(double f_0,double f_1,double f_2)
lokitsyu 1:5c6888eb051a 78 {
lokitsyu 2:906c20c9d15e 79 sum += (f_0 + 4 * f_1 + f_2)/6000;
lokitsyu 2:906c20c9d15e 80 return sum;
lokitsyu 0:3137e34fc9fa 81 }
lokitsyu 0:3137e34fc9fa 82
lokitsyu 0:3137e34fc9fa 83
lokitsyu 1:5c6888eb051a 84 ///////////////////////////////////////////////////////////////////////
lokitsyu 1:5c6888eb051a 85 //motor drive control funciton
lokitsyu 1:5c6888eb051a 86 ///////////////////////////////////////////////////////////////////////
lokitsyu 1:5c6888eb051a 87 void motordrv()
lokitsyu 1:5c6888eb051a 88 {
lokitsyu 1:5c6888eb051a 89 for(int i=0;i<4;i++)
lokitsyu 1:5c6888eb051a 90 {
lokitsyu 1:5c6888eb051a 91 i2c.start();
lokitsyu 1:5c6888eb051a 92 i2c.write(addr[i],outval[i],1);
lokitsyu 1:5c6888eb051a 93 i2c.stop();
lokitsyu 1:5c6888eb051a 94 }
lokitsyu 1:5c6888eb051a 95 }
lokitsyu 0:3137e34fc9fa 96
lokitsyu 1:5c6888eb051a 97 ///////////////////////////////////////////////////////////////////////
lokitsyu 0:3137e34fc9fa 98 //main function
lokitsyu 1:5c6888eb051a 99 ///////////////////////////////////////////////////////////////////////
lokitsyu 0:3137e34fc9fa 100 int main() {
lokitsyu 1:5c6888eb051a 101 Gyro gyro;
lokitsyu 0:3137e34fc9fa 102
lokitsyu 1:5c6888eb051a 103 i2c.frequency(400000);
lokitsyu 1:5c6888eb051a 104 unit.mode(PullUp);
lokitsyu 1:5c6888eb051a 105 unit.fall(&gyro,&Gyro::get);
lokitsyu 1:5c6888eb051a 106 timer.attach_us(&motordrv,10000);
lokitsyu 0:3137e34fc9fa 107
lokitsyu 1:5c6888eb051a 108 gyro.set();
lokitsyu 0:3137e34fc9fa 109
lokitsyu 0:3137e34fc9fa 110 //main loop
lokitsyu 0:3137e34fc9fa 111 while(1) {
lokitsyu 1:5c6888eb051a 112 gyro.calc();
lokitsyu 1:5c6888eb051a 113 /*
lokitsyu 0:3137e34fc9fa 114
lokitsyu 0:3137e34fc9fa 115
lokitsyu 1:5c6888eb051a 116 Write Program on This Space
lokitsyu 1:5c6888eb051a 117
lokitsyu 0:3137e34fc9fa 118
lokitsyu 1:5c6888eb051a 119 */
lokitsyu 0:3137e34fc9fa 120 }
lokitsyu 0:3137e34fc9fa 121 }