kakunin
Dependencies: ADXL345 HMC6352 Motor PID SoftPWM mbed
main.cpp
- Committer:
- WAT34
- Date:
- 2015-07-12
- Revision:
- 6:5d77f1347f79
- Parent:
- 5:ec9f341817ef
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; } } }