mbedを用いた制御学生の制御 / Mbed 2 deprecated AIR

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?

UserRevisionLine numberNew 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 }