File content as of revision 6:5d77f1347f79:

/*========================
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
#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"
#include "HMC6352.h"
#include "SoftPWM.h"
Timeout sho;
Timeout rev;
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};

    sp1.period(1.0/mm[0]);
    sp1.write(0.5f);
}
void shoot()
{
    if (sw == 0) {
        air = 1;
    }
    sp1.write(0.0f);
    c = 0;
    wait(1);
}
void ret()
{
    if (sw2 == 0) {
        air = 2;
    }
    wait(1);
    sp1.write(0.0f);
    c = 0;
}
int main()
{
    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);

    //データレートを3200Hzに設定。
    accell.setDataRate(ADXL345_3200HZ);

    //計測モード設定
    accell.setPowerControl(0x08);
    //====方位センサHMC6352の設定=======
    mag.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //方位の基準を取得
    da = mag.sample()/10.0;
//===========無限ループ========================
    while(1) {
        //加速度センサの値で角度を調整。
        accell.getOutput(gd);
        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();
            sho.attach(&shoot,3.0);
            c = 1;
        } else if(sw2 == 0 && c == 0) {
            sound();
            rev.attach(&ret,3.0);
            c = 1;
        } else {
            air = 0;
        }
    }
}