File content as of revision 2:b8e04b6dc9ab:

/*========================
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"
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 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()
{
    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],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);

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

    //計測モード設定
    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();
        //方位センサの値で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) {
            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;
        }
    }
}