Diff: main.cpp

Revision:

4:bd8e35d546d7

Parent:

3:80c921093b4d

--- a/main.cpp	Wed Jul 08 11:05:16 2015 +0000
+++ b/main.cpp	Sat Jul 11 05:18:44 2015 +0000
@@ -18,14 +18,14 @@
 #include "SoftPWM.h"
 Timeout sho;
 Timeout rev;
-SoftPWM sp1(dp25);
+Serial pc(dp16,NC);
 BusOut air(dp9,dp10);
-BusOut air2(dp11,dp13);
+SoftPWM sp1(dp11);
 DigitalIn sw(dp28,PullUp);
 DigitalIn sw2(dp26,PullUp);
-ADXL345 accell(dp1,dp2,dp6,dp4);
+ADXL345 accell(dp2,dp1,dp6,dp4);
 PID tilt(0.1, 0.0, 0.0, 0.1);
-Motor motort(dp24,dp17,dp16);
+Motor motort(dp24,dp17,dp25);
 Motor motord(dp18,dp15,dp14);
 HMC6352 mag (dp5,dp27);
 PID direct(0.1, 0.0, 0.0, 0.1);
@@ -41,7 +41,6 @@
 {
     if (sw == 0) {
         air = 1;
-        air2 = 1;
     }
     sp1.write(0.0f);
     c = 0;
@@ -51,7 +50,6 @@
 {
     if (sw2 == 0) {
         air = 2;
-        air2 = 2;
     }
     wait(1);
     sp1.write(0.0f);
@@ -59,7 +57,7 @@
 }
 int main()
 {
-    int gd[3],degree;
+    int degree,gd[3];
     float tc,dc,deg,da;
     double x,y,z;
     //====加速度センサの準備============
@@ -88,21 +86,21 @@
     direct.setOutputLimits(-1.0,1.0);
     direct.setMode(AUTO_MODE);
     //方位の基準を取得
-    da = mag.sample();
+    da = mag.sample()/10.0;
 //===========無限ループ========================
     while(1) {
         //加速度センサの値で角度を調整。
         accell.getOutput(gd);
-        x = gd[0]/4*0.001;
-        y = gd[1]/4*0.001;
-        z = gd[2]/4*0.001;
+        x = int16_t(gd[0])/4*0.001;
+        y = int16_t(gd[1])/4*0.001;
+        z = int16_t(gd[2])/4*0.001;
         //各PIDの目標値を設定。
-        tilt.setSetPoint(1.7);
+        tilt.setSetPoint(0);
         direct.setSetPoint(1.7);
-        tilt.setProcessValue(atan2(x,y));
+        tilt.setProcessValue(atan2(x,z));
         tc = tilt.compute();
         //方位センサの値でPID制御
-        deg = mag.sample();
+        deg = mag.sample()/10.0;
         degree = deg+540-da;
         degree %= 360;
         degree -= 180;
@@ -110,7 +108,7 @@
         dc = direct.compute();
         motort.speed(tc);
         motord.speed(dc);
-        printf("degree%d\n\r",degree);
+        pc.printf("degree %d  x %f\n\r",degree,tilt.compute());
         
         //発射官制
         if (sw == 0 && c == 0) {
@@ -123,7 +121,6 @@
             c = 1;
         } else {
             air = 0;
-            air2 = 0;
         }
     }
 }