Revision 6:5d77f1347f79, committed 2015-07-12

Comitter:

WAT34

Date:

Sun Jul 12 01:20:07 2015 +0000

Parent:

5:ec9f341817ef

Commit message:

ugoku?;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Sun Jul 12 01:20:07 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

--- a/PID.lib	Sun Jul 12 00:35:11 2015 +0000
+++ b/PID.lib	Sun Jul 12 01:20:07 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
+http://developer.mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SoftPWM.lib	Sun Jul 12 01:20:07 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/komaida424/code/SoftPWM/#7918ce37626c

--- a/main.cpp	Sun Jul 12 00:35:11 2015 +0000
+++ b/main.cpp	Sun Jul 12 01:20:07 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,17 +14,20 @@
 #include "ADXL345.h"
 #include "PID.h"
 #include "Motor.h"
+#include "HMC6352.h"
+#include "SoftPWM.h"
 Timeout sho;
 Timeout rev;
-PwmOut sp1(p25);
-BusOut air(p15,p16);
-BusOut air2(p17,p18);
-DigitalIn sw(p10,PullUp);
-DigitalIn sw2(p11,PullUp);
-ADXL345 accell(p5,p6,p7,p8);
-PID tilt(0.1, 0.0, 0.0, 0.1);
-Motor motor(p21,p12,p13);
-int c;
+Serial pc(dp16,NC);
+BusOut air(dp9,dp10);
+SoftPWM sp1(dp11);
+DigitalIn sw(dp28,PullUp);
+DigitalIn sw2(dp26,PullUp);
+ADXL345 accell(dp2,dp1,dp6,dp4);
+Motor motort(dp24,dp17,dp25);
+Motor motord(dp18,dp15,dp14);
+HMC6352 mag (dp5,dp27);
+int c = 0;
 void sound()
 {
     float mm[]= {mC,mD,mE,mF,mG,mA,mB,mC*2};
@@ -33,7 +39,6 @@
 {
     if (sw == 0) {
         air = 1;
-        air2 = 1;
     }
     sp1.write(0.0f);
     c = 0;
@@ -43,7 +48,6 @@
 {
     if (sw2 == 0) {
         air = 2;
-        air2 = 2;
     }
     wait(1);
     sp1.write(0.0f);
@@ -51,13 +55,12 @@
 }
 int main()
 {
-    int gd[3];
-    float tc;
+    int degree,gd[3];
+    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,23 +69,25 @@
 
     //計測モード設定
     accell.setPowerControl(0x08);
-    //====発射角度PID設定===============
-    //角度センサの値の範囲
-    tilt.setInputLimits(PI,-PI);
-    //出力の設定
-    tilt.setOutputLimits(-1.0,1.0);
-    tilt.setMode(AUTO_MODE);
+    //====方位センサHMC6352の設定=======
+    mag.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    //方位の基準を取得
+    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;
-        tilt.setProcessValue(atan2(x,y));
-        tc = tilt.compute();
-        motor.speed(tc);
-        printf("%f\n\r",atan2(x,y));
-        
+        x = int16_t(gd[0])/4*0.001;
+        y = int16_t(gd[1])/4*0.001;
+        z = int16_t(gd[2])/4*0.001;
+        //偏差を算出
+        deg = mag.sample()/10.0;
+        degree = deg+540-da;
+        degree %= 360;
+        degree -= 180;
+        motort.speed(atan2(x,z)*0.1);
+        motord.speed(dc);
+        pc.printf("degree %d  x %d\n\r",degree,degree);
         //発射官制
         if (sw == 0 && c == 0) {
             sound();
@@ -94,7 +99,6 @@
             c = 1;
         } else {
             air = 0;
-            air2 = 0;
         }
     }
 }