Diff: main.cpp

Revision:

1:56dc085d9e2d

Parent:

0:7ec77e777058

Child:

2:b8e04b6dc9ab

Child:

5:ec9f341817ef

--- a/main.cpp	Wed Jul 01 07:17:03 2015 +0000
+++ b/main.cpp	Sun Jul 05 08:18:46 2015 +0000
@@ -1,18 +1,98 @@
 #include "mbed.h"
-
+#define mC 3920.072
+#define mD 293.665
+#define mE 329.628
+#define mF 349.228
+#define mG 391.995
+#define mA 440.000
+#define mB 493.883
+#define PI 3.14159265358979
+#include "mbed.h"
+#include "ADXL345.h"
+#include "PID.h"
+#include "Motor.h"
+Timeout sho;
+Timeout rev;
+PwmOut sp1(p25);
 BusOut air(p15,p16);
 BusOut air2(p17,p18);
 DigitalIn sw(p10,PullUp);
 DigitalIn sw2(p11,PullUp);
-int main() {
+ADXL345 accell(p5,p6,p7,p8);
+PID tilt(0.1, 0.0, 0.0, 0.1);
+Motor motor(p12,p13,p21);
+int c;
+void sound()
+{
+    float mm[]= {mC,mD,mE,mF,mG,mA,mB,mC*2};
+
+    sp1.period(1.0/mm[0]);
+    sp1.write(0.5f);
+}
+void shoot()
+{
+    if (sw == 0) {
+        air = 1;
+        air2 = 1;
+    }
+    sp1.write(0.0f);
+    c = 0;
+    wait(1);
+}
+void ret()
+{
+    if (sw2 == 0) {
+        air = 2;
+        air2 = 2;
+    }
+    wait(1);
+    sp1.write(0.0f);
+    c = 0;
+}
+int main()
+{
+    int gd[3];
+    float tc;
+    double x,y,z;
+    //====加速度センサの準備============
+    //Go into standby mode to configure the device.
+    accell.setPowerControl(0x00);
+
+    //設定, +/-16g, 4mg/LSB.
+    accell.setDataFormatControl(0x0B);
+
+    //データレートを3200Hzに設定。
+    accell.setDataRate(ADXL345_3200HZ);
+
+    //計測モード設定
+    accell.setPowerControl(0x08);
+    //====発射角度PID設定===============
+    //角度センサの値の範囲
+    tilt.setInputLimits(PI,-PI);
+    //出力の設定
+    tilt.setOutputLimits(-1.0,1.0);
+    tilt.setMode(AUTO_MODE);
     while(1) {
-        if (sw == 0){
-            air = 1;
-            air2 = 1;
-        }else if(sw2 == 0){
-            air =2;
-            air2 = 2;
-        }else{
+        //加速度センサの値で角度を調整。
+        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));
+        
+        //発射官制
+        if (sw == 0 && c == 0) {
+            sound();
+            sho.attach(&shoot,3.0);
+            c = 1;
+        } else if(sw2 == 0 && c == 0) {
+            sound();
+            rev.attach(&ret,3.0);
+            c = 1;
+        } else {
             air = 0;
             air2 = 0;
         }