Diff: main.cpp

Revision:

2:b8e04b6dc9ab

Parent:

1:56dc085d9e2d

Child:

3:80c921093b4d

--- a/main.cpp	Sun Jul 05 08:18:46 2015 +0000
+++ b/main.cpp	Tue Jul 07 07:40:06 2015 +0000
@@ -1,4 +1,7 @@
-#include "mbed.h"
+/*========================
+2015/07/05 Author Wtaru Nakata
+Controlling air with solenoid valve and tiltness with accelamater for nhk robocon2015.
+==========================*/ 
 #define mC 3920.072
 #define mD 293.665
 #define mE 329.628
@@ -11,6 +14,7 @@
 #include "ADXL345.h"
 #include "PID.h"
 #include "Motor.h"
+#include "HMC6352.h"
 Timeout sho;
 Timeout rev;
 PwmOut sp1(p25);
@@ -20,7 +24,10 @@
 DigitalIn sw2(p11,PullUp);
 ADXL345 accell(p5,p6,p7,p8);
 PID tilt(0.1, 0.0, 0.0, 0.1);
-Motor motor(p12,p13,p21);
+Motor motort(p21,p12,p13);
+Motor motord(p22,p27,p28);
+HMC6352 mag (p9,p10);
+PID direct(0.1, 0.0, 0.0, 0.1);
 int c;
 void sound()
 {
@@ -51,13 +58,12 @@
 }
 int main()
 {
-    int gd[3];
-    float tc;
+    int gd[3],degree;
+    float tc,dc,deg,da;
     double x,y,z;
     //====加速度センサの準備============
     //Go into standby mode to configure the device.
     accell.setPowerControl(0x00);
-
     //設定, +/-16g, 4mg/LSB.
     accell.setDataFormatControl(0x0B);
 
@@ -66,22 +72,44 @@
 
     //計測モード設定
     accell.setPowerControl(0x08);
+    //====方位センサHMC6352の設定=======
+    mag.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     //====発射角度PID設定===============
     //角度センサの値の範囲
     tilt.setInputLimits(PI,-PI);
     //出力の設定
     tilt.setOutputLimits(-1.0,1.0);
     tilt.setMode(AUTO_MODE);
+    //向きの設定
+    //方位センサの値の範囲
+    direct.setInputLimits(180,-180);
+    //====出力の設定==================
+    direct.setOutputLimits(-1.0,1.0);
+    direct.setMode(AUTO_MODE);
+    //方位の基準を取得
+    da = mag.sample();
+//===========無限ループ========================
     while(1) {
         //加速度センサの値で角度を調整。
         accell.getOutput(gd);
         x = gd[0]/4*0.001;
         y = gd[1]/4*0.001;
         z = gd[2]/4*0.001;
+        //各PIDの目標値を設定。
+        tilt.setSetPoint(1.7);
+        direct.setSetPoint(1.7);
         tilt.setProcessValue(atan2(x,y));
         tc = tilt.compute();
-        motor.speed(tc);
-        printf("%f\n\r",atan2(x,y));
+        //方位センサの値でPID制御
+        deg = mag.sample();
+        degree = deg+540-da;
+        degree %= 360;
+        degree -= 180;
+        direct.setProcessValue(degree);
+        dc = direct.compute();
+        motort.speed(tc);
+        motord.speed(dc);
+        printf("degree%d\n\r",degree);
         
         //発射官制
         if (sw == 0 && c == 0) {