main.cpp

00001 /*========================
00002 2015/07/05 Author Wtaru Nakata
00003 Controlling air with solenoid valve and tiltness with accelamater for nhk robocon2015.
00004 ==========================*/ 
00005 #define mC 3920.072
00006 #define mD 293.665
00007 #define mE 329.628
00008 #define mF 349.228
00009 #define mG 391.995
00010 #define mA 440.000
00011 #define mB 493.883
00012 #define PI 3.14159265358979
00013 #include "mbed.h"
00014 #include "ADXL345.h"
00015 #include "PID.h"
00016 #include "Motor.h"
00017 #include "HMC6352.h"
00018 #include "SoftPWM.h"
00019 Timeout sho;
00020 Timeout rev;
00021 Serial pc(dp16,NC);
00022 BusOut air(dp9,dp10);
00023 SoftPWM sp1(dp11);
00024 DigitalIn sw(dp28,PullUp);
00025 DigitalIn sw2(dp26,PullUp);
00026 ADXL345 accell(dp2,dp1,dp6,dp4);
00027 Motor motort(dp24,dp17,dp25);
00028 Motor motord(dp18,dp15,dp14);
00029 HMC6352 mag (dp5,dp27);
00030 int c = 0;
00031 void sound()
00032 {
00033     float mm[]= {mC,mD,mE,mF,mG,mA,mB,mC*2};
00034 
00035     sp1.period(1.0/mm[0]);
00036     sp1.write(0.5f);
00037 }
00038 void shoot()
00039 {
00040     if (sw == 0) {
00041         air = 1;
00042     }
00043     sp1.write(0.0f);
00044     c = 0;
00045     wait(1);
00046 }
00047 void ret()
00048 {
00049     if (sw2 == 0) {
00050         air = 2;
00051     }
00052     wait(1);
00053     sp1.write(0.0f);
00054     c = 0;
00055 }
00056 int main()
00057 {
00058     int degree,gd[3];
00059     float tc,dc,deg,da;
00060     double x,y,z;
00061     //====加速度センサの準備============
00062     //Go into standby mode to configure the device.
00063     accell.setPowerControl(0x00);
00064     //設定, +/-16g, 4mg/LSB.
00065     accell.setDataFormatControl(0x0B);
00066 
00067     //データレートを3200Hzに設定。
00068     accell.setDataRate(ADXL345_3200HZ);
00069 
00070     //計測モード設定
00071     accell.setPowerControl(0x08);
00072     //====方位センサHMC6352の設定=======
00073     mag.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00074     //方位の基準を取得
00075     da = mag.sample()/10.0;
00076 //===========無限ループ========================
00077     while(1) {
00078         //加速度センサの値で角度を調整。
00079         accell.getOutput(gd);
00080         x = int16_t(gd[0])/4*0.001;
00081         y = int16_t(gd[1])/4*0.001;
00082         z = int16_t(gd[2])/4*0.001;
00083         //偏差を算出
00084         deg = mag.sample()/10.0;
00085         degree = deg+540-da;
00086         degree %= 360;
00087         degree -= 180;
00088         motort.speed(atan2(x,z)*0.1);
00089         motord.speed(dc);
00090         pc.printf("degree %d  x %d\n\r",degree,degree);
00091         //発射官制
00092         if (sw == 0 && c == 0) {
00093             sound();
00094             sho.attach(&shoot,3.0);
00095             c = 1;
00096         } else if(sw2 == 0 && c == 0) {
00097             sound();
00098             rev.attach(&ret,3.0);
00099             c = 1;
00100         } else {
00101             air = 0;
00102         }
00103     }
00104 }