Diff: main.cpp

Revision:

0:cb965cafb758

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 29 15:00:26 2015 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#define pi 3.141592653589794
+#define waddr 0xD0
+#define raddr 0xD1
+#define kt 0.1  //出力値の各ゲインを調節
+#define ko 0.1
+#define kp 0.1
+#define kd 0.1
+PwmOut mp[4] ={p21,p22,p23,p24};
+I2C sen(p9,p10);
+Timer global;
+Timer pt;
+void motor_cont(char select,double percent)//selectが0の時は左がselectが１の時は右のモータをコントロール。
+{
+    select *= 2;
+    if (percent>0){
+        mp[select] = fabs(percent);
+        mp[select+1] = 0;
+    }else if (percent <0){
+        mp[select] = 0;
+        mp[select+1] = fabs(percent);
+    }else{
+        mp[select] = 0;
+        mp[select+1] = 0;
+    }
+}
+void sensor_init(){ //センサ計測よーーーーーい
+     sen.frequency(400000);
+     char put[2][2]; //送る値の用意
+     put[0][0] = 0x6b;
+     put[0][1] = 0x00;
+     put[1][0] = 0x37;
+     put[1][1] = 0x02;
+     sen.write(waddr,put[0],2); //送信
+     sen.write(waddr,put[1],2);
+}
+void get_gyro(double *x,double *y,double *z,int *time) //ジャイロの値をもってくる
+{
+    int i; //変数用意
+    char tv[6];
+    short t[3];
+    char put[] ={0x43};
+    sen.write(waddr,put,1); //センサに送信
+    sen.read(raddr,tv,6);
+    for(i = 0;i <3;i++){ //xyzそれぞれの値を算出
+        t[i] = tv[2*i]<<8+tv[2*i+1];
+    }
+    global.stop();
+    *time = global.read_us();
+    global.reset();
+    global.start();
+    *x = t[0]*0.00763; //dag/secに単位換算
+    *y = t[1]*0.00763;
+    *z = t[2]*0.00763;
+}
+    
+//------------------main function------------------------------------------------------------------------------------------
+int main() {
+    global.start(); //プログラムでの経過時間をけいそく（積分などで利用）
+    sensor_init();
+    double x,y,z,x2,theta,omega,power,power2,distance;
+    int time,time2;
+    while(1) {
+        get_gyro(&x,&y,&z,&time);
+        theta = theta+(x+x2)*time/(2.0*1000000.0); //積分して角度算出
+        omega = x;
+        power = kt*theta + ko*omega +kp*power;
+        time2 = pt.read_us();
+        distance = distance + (power+power2)*time2/(2.0*1000000.0); //移動距離を出力を積分して算出
+        power += kd*distance;
+        pt.reset();
+        pt.start();
+        x2 = x;
+        power2 = power;
+    }
+}