kakunin
Dependencies: ADXL345 HMC6352 Motor PID SoftPWM mbed
main.cpp@6:5d77f1347f79, 2015-07-12 (annotated)
- Committer:
- WAT34
- Date:
- Sun Jul 12 01:20:07 2015 +0000
- Revision:
- 6:5d77f1347f79
- Parent:
- 5:ec9f341817ef
ugoku?;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 6:5d77f1347f79 | 1 | /*======================== |
WAT34 | 6:5d77f1347f79 | 2 | 2015/07/05 Author Wtaru Nakata |
WAT34 | 6:5d77f1347f79 | 3 | Controlling air with solenoid valve and tiltness with accelamater for nhk robocon2015. |
WAT34 | 6:5d77f1347f79 | 4 | ==========================*/ |
WAT34 | 1:56dc085d9e2d | 5 | #define mC 3920.072 |
WAT34 | 1:56dc085d9e2d | 6 | #define mD 293.665 |
WAT34 | 1:56dc085d9e2d | 7 | #define mE 329.628 |
WAT34 | 1:56dc085d9e2d | 8 | #define mF 349.228 |
WAT34 | 1:56dc085d9e2d | 9 | #define mG 391.995 |
WAT34 | 1:56dc085d9e2d | 10 | #define mA 440.000 |
WAT34 | 1:56dc085d9e2d | 11 | #define mB 493.883 |
WAT34 | 1:56dc085d9e2d | 12 | #define PI 3.14159265358979 |
WAT34 | 1:56dc085d9e2d | 13 | #include "mbed.h" |
WAT34 | 1:56dc085d9e2d | 14 | #include "ADXL345.h" |
WAT34 | 1:56dc085d9e2d | 15 | #include "PID.h" |
WAT34 | 1:56dc085d9e2d | 16 | #include "Motor.h" |
WAT34 | 6:5d77f1347f79 | 17 | #include "HMC6352.h" |
WAT34 | 6:5d77f1347f79 | 18 | #include "SoftPWM.h" |
WAT34 | 1:56dc085d9e2d | 19 | Timeout sho; |
WAT34 | 1:56dc085d9e2d | 20 | Timeout rev; |
WAT34 | 6:5d77f1347f79 | 21 | Serial pc(dp16,NC); |
WAT34 | 6:5d77f1347f79 | 22 | BusOut air(dp9,dp10); |
WAT34 | 6:5d77f1347f79 | 23 | SoftPWM sp1(dp11); |
WAT34 | 6:5d77f1347f79 | 24 | DigitalIn sw(dp28,PullUp); |
WAT34 | 6:5d77f1347f79 | 25 | DigitalIn sw2(dp26,PullUp); |
WAT34 | 6:5d77f1347f79 | 26 | ADXL345 accell(dp2,dp1,dp6,dp4); |
WAT34 | 6:5d77f1347f79 | 27 | Motor motort(dp24,dp17,dp25); |
WAT34 | 6:5d77f1347f79 | 28 | Motor motord(dp18,dp15,dp14); |
WAT34 | 6:5d77f1347f79 | 29 | HMC6352 mag (dp5,dp27); |
WAT34 | 6:5d77f1347f79 | 30 | int c = 0; |
WAT34 | 1:56dc085d9e2d | 31 | void sound() |
WAT34 | 1:56dc085d9e2d | 32 | { |
WAT34 | 1:56dc085d9e2d | 33 | float mm[]= {mC,mD,mE,mF,mG,mA,mB,mC*2}; |
WAT34 | 1:56dc085d9e2d | 34 | |
WAT34 | 1:56dc085d9e2d | 35 | sp1.period(1.0/mm[0]); |
WAT34 | 1:56dc085d9e2d | 36 | sp1.write(0.5f); |
WAT34 | 1:56dc085d9e2d | 37 | } |
WAT34 | 1:56dc085d9e2d | 38 | void shoot() |
WAT34 | 1:56dc085d9e2d | 39 | { |
WAT34 | 1:56dc085d9e2d | 40 | if (sw == 0) { |
WAT34 | 1:56dc085d9e2d | 41 | air = 1; |
WAT34 | 1:56dc085d9e2d | 42 | } |
WAT34 | 1:56dc085d9e2d | 43 | sp1.write(0.0f); |
WAT34 | 1:56dc085d9e2d | 44 | c = 0; |
WAT34 | 1:56dc085d9e2d | 45 | wait(1); |
WAT34 | 1:56dc085d9e2d | 46 | } |
WAT34 | 1:56dc085d9e2d | 47 | void ret() |
WAT34 | 1:56dc085d9e2d | 48 | { |
WAT34 | 1:56dc085d9e2d | 49 | if (sw2 == 0) { |
WAT34 | 1:56dc085d9e2d | 50 | air = 2; |
WAT34 | 1:56dc085d9e2d | 51 | } |
WAT34 | 1:56dc085d9e2d | 52 | wait(1); |
WAT34 | 1:56dc085d9e2d | 53 | sp1.write(0.0f); |
WAT34 | 1:56dc085d9e2d | 54 | c = 0; |
WAT34 | 1:56dc085d9e2d | 55 | } |
WAT34 | 1:56dc085d9e2d | 56 | int main() |
WAT34 | 1:56dc085d9e2d | 57 | { |
WAT34 | 6:5d77f1347f79 | 58 | int degree,gd[3]; |
WAT34 | 6:5d77f1347f79 | 59 | float tc,dc,deg,da; |
WAT34 | 1:56dc085d9e2d | 60 | double x,y,z; |
WAT34 | 1:56dc085d9e2d | 61 | //====加速度センサの準備============ |
WAT34 | 1:56dc085d9e2d | 62 | //Go into standby mode to configure the device. |
WAT34 | 1:56dc085d9e2d | 63 | accell.setPowerControl(0x00); |
WAT34 | 1:56dc085d9e2d | 64 | //設定, +/-16g, 4mg/LSB. |
WAT34 | 1:56dc085d9e2d | 65 | accell.setDataFormatControl(0x0B); |
WAT34 | 1:56dc085d9e2d | 66 | |
WAT34 | 1:56dc085d9e2d | 67 | //データレートを3200Hzに設定。 |
WAT34 | 1:56dc085d9e2d | 68 | accell.setDataRate(ADXL345_3200HZ); |
WAT34 | 1:56dc085d9e2d | 69 | |
WAT34 | 1:56dc085d9e2d | 70 | //計測モード設定 |
WAT34 | 1:56dc085d9e2d | 71 | accell.setPowerControl(0x08); |
WAT34 | 6:5d77f1347f79 | 72 | //====方位センサHMC6352の設定======= |
WAT34 | 6:5d77f1347f79 | 73 | mag.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
WAT34 | 6:5d77f1347f79 | 74 | //方位の基準を取得 |
WAT34 | 6:5d77f1347f79 | 75 | da = mag.sample()/10.0; |
WAT34 | 6:5d77f1347f79 | 76 | //===========無限ループ======================== |
WAT34 | 0:7ec77e777058 | 77 | while(1) { |
WAT34 | 1:56dc085d9e2d | 78 | //加速度センサの値で角度を調整。 |
WAT34 | 1:56dc085d9e2d | 79 | accell.getOutput(gd); |
WAT34 | 6:5d77f1347f79 | 80 | x = int16_t(gd[0])/4*0.001; |
WAT34 | 6:5d77f1347f79 | 81 | y = int16_t(gd[1])/4*0.001; |
WAT34 | 6:5d77f1347f79 | 82 | z = int16_t(gd[2])/4*0.001; |
WAT34 | 6:5d77f1347f79 | 83 | //偏差を算出 |
WAT34 | 6:5d77f1347f79 | 84 | deg = mag.sample()/10.0; |
WAT34 | 6:5d77f1347f79 | 85 | degree = deg+540-da; |
WAT34 | 6:5d77f1347f79 | 86 | degree %= 360; |
WAT34 | 6:5d77f1347f79 | 87 | degree -= 180; |
WAT34 | 6:5d77f1347f79 | 88 | motort.speed(atan2(x,z)*0.1); |
WAT34 | 6:5d77f1347f79 | 89 | motord.speed(dc); |
WAT34 | 6:5d77f1347f79 | 90 | pc.printf("degree %d x %d\n\r",degree,degree); |
WAT34 | 1:56dc085d9e2d | 91 | //発射官制 |
WAT34 | 1:56dc085d9e2d | 92 | if (sw == 0 && c == 0) { |
WAT34 | 1:56dc085d9e2d | 93 | sound(); |
WAT34 | 1:56dc085d9e2d | 94 | sho.attach(&shoot,3.0); |
WAT34 | 1:56dc085d9e2d | 95 | c = 1; |
WAT34 | 1:56dc085d9e2d | 96 | } else if(sw2 == 0 && c == 0) { |
WAT34 | 1:56dc085d9e2d | 97 | sound(); |
WAT34 | 1:56dc085d9e2d | 98 | rev.attach(&ret,3.0); |
WAT34 | 1:56dc085d9e2d | 99 | c = 1; |
WAT34 | 1:56dc085d9e2d | 100 | } else { |
WAT34 | 0:7ec77e777058 | 101 | air = 0; |
WAT34 | 0:7ec77e777058 | 102 | } |
WAT34 | 0:7ec77e777058 | 103 | } |
WAT34 | 0:7ec77e777058 | 104 | } |