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

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ADXL345 HMC6352 Motor PID SoftPWM mbed

main.cpp@2:b8e04b6dc9ab, 2015-07-07 (annotated)

Committer:
WAT34
Date:
Tue Jul 07 07:40:06 2015 +0000
Revision:
2:b8e04b6dc9ab
Parent:
1:56dc085d9e2d
Child:
3:80c921093b4d
???;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 2:b8e04b6dc9ab 1 /*========================
WAT34 2:b8e04b6dc9ab 2 2015/07/05 Author Wtaru Nakata
WAT34 2:b8e04b6dc9ab 3 Controlling air with solenoid valve and tiltness with accelamater for nhk robocon2015.
WAT34 2:b8e04b6dc9ab 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 2:b8e04b6dc9ab 17 #include "HMC6352.h"
WAT34 1:56dc085d9e2d 18 Timeout sho;
WAT34 1:56dc085d9e2d 19 Timeout rev;
WAT34 1:56dc085d9e2d 20 PwmOut sp1(p25);
WAT34 0:7ec77e777058 21 BusOut air(p15,p16);
WAT34 0:7ec77e777058 22 BusOut air2(p17,p18);
WAT34 0:7ec77e777058 23 DigitalIn sw(p10,PullUp);
WAT34 0:7ec77e777058 24 DigitalIn sw2(p11,PullUp);
WAT34 1:56dc085d9e2d 25 ADXL345 accell(p5,p6,p7,p8);
WAT34 1:56dc085d9e2d 26 PID tilt(0.1, 0.0, 0.0, 0.1);
WAT34 2:b8e04b6dc9ab 27 Motor motort(p21,p12,p13);
WAT34 2:b8e04b6dc9ab 28 Motor motord(p22,p27,p28);
WAT34 2:b8e04b6dc9ab 29 HMC6352 mag (p9,p10);
WAT34 2:b8e04b6dc9ab 30 PID direct(0.1, 0.0, 0.0, 0.1);
WAT34 1:56dc085d9e2d 31 int c;
WAT34 1:56dc085d9e2d 32 void sound()
WAT34 1:56dc085d9e2d 33 {
WAT34 1:56dc085d9e2d 34 float mm[]= {mC,mD,mE,mF,mG,mA,mB,mC*2};
WAT34 1:56dc085d9e2d 35
WAT34 1:56dc085d9e2d 36 sp1.period(1.0/mm[0]);
WAT34 1:56dc085d9e2d 37 sp1.write(0.5f);
WAT34 1:56dc085d9e2d 38 }
WAT34 1:56dc085d9e2d 39 void shoot()
WAT34 1:56dc085d9e2d 40 {
WAT34 1:56dc085d9e2d 41 if (sw == 0) {
WAT34 1:56dc085d9e2d 42 air = 1;
WAT34 1:56dc085d9e2d 43 air2 = 1;
WAT34 1:56dc085d9e2d 44 }
WAT34 1:56dc085d9e2d 45 sp1.write(0.0f);
WAT34 1:56dc085d9e2d 46 c = 0;
WAT34 1:56dc085d9e2d 47 wait(1);
WAT34 1:56dc085d9e2d 48 }
WAT34 1:56dc085d9e2d 49 void ret()
WAT34 1:56dc085d9e2d 50 {
WAT34 1:56dc085d9e2d 51 if (sw2 == 0) {
WAT34 1:56dc085d9e2d 52 air = 2;
WAT34 1:56dc085d9e2d 53 air2 = 2;
WAT34 1:56dc085d9e2d 54 }
WAT34 1:56dc085d9e2d 55 wait(1);
WAT34 1:56dc085d9e2d 56 sp1.write(0.0f);
WAT34 1:56dc085d9e2d 57 c = 0;
WAT34 1:56dc085d9e2d 58 }
WAT34 1:56dc085d9e2d 59 int main()
WAT34 1:56dc085d9e2d 60 {
WAT34 2:b8e04b6dc9ab 61 int gd[3],degree;
WAT34 2:b8e04b6dc9ab 62 float tc,dc,deg,da;
WAT34 1:56dc085d9e2d 63 double x,y,z;
WAT34 1:56dc085d9e2d 64 //====加速度センサの準備============
WAT34 1:56dc085d9e2d 65 //Go into standby mode to configure the device.
WAT34 1:56dc085d9e2d 66 accell.setPowerControl(0x00);
WAT34 1:56dc085d9e2d 67 //設定, +/-16g, 4mg/LSB.
WAT34 1:56dc085d9e2d 68 accell.setDataFormatControl(0x0B);
WAT34 1:56dc085d9e2d 69
WAT34 1:56dc085d9e2d 70 //データレートを3200Hzに設定。
WAT34 1:56dc085d9e2d 71 accell.setDataRate(ADXL345_3200HZ);
WAT34 1:56dc085d9e2d 72
WAT34 1:56dc085d9e2d 73 //計測モード設定
WAT34 1:56dc085d9e2d 74 accell.setPowerControl(0x08);
WAT34 2:b8e04b6dc9ab 75 //====方位センサHMC6352の設定=======
WAT34 2:b8e04b6dc9ab 76 mag.setOpMode(HMC6352_CONTINUOUS, 1, 20);
WAT34 1:56dc085d9e2d 77 //====発射角度PID設定===============
WAT34 1:56dc085d9e2d 78 //角度センサの値の範囲
WAT34 1:56dc085d9e2d 79 tilt.setInputLimits(PI,-PI);
WAT34 1:56dc085d9e2d 80 //出力の設定
WAT34 1:56dc085d9e2d 81 tilt.setOutputLimits(-1.0,1.0);
WAT34 1:56dc085d9e2d 82 tilt.setMode(AUTO_MODE);
WAT34 2:b8e04b6dc9ab 83 //向きの設定
WAT34 2:b8e04b6dc9ab 84 //方位センサの値の範囲
WAT34 2:b8e04b6dc9ab 85 direct.setInputLimits(180,-180);
WAT34 2:b8e04b6dc9ab 86 //====出力の設定==================
WAT34 2:b8e04b6dc9ab 87 direct.setOutputLimits(-1.0,1.0);
WAT34 2:b8e04b6dc9ab 88 direct.setMode(AUTO_MODE);
WAT34 2:b8e04b6dc9ab 89 //方位の基準を取得
WAT34 2:b8e04b6dc9ab 90 da = mag.sample();
WAT34 2:b8e04b6dc9ab 91 //===========無限ループ========================
WAT34 0:7ec77e777058 92 while(1) {
WAT34 1:56dc085d9e2d 93 //加速度センサの値で角度を調整。
WAT34 1:56dc085d9e2d 94 accell.getOutput(gd);
WAT34 1:56dc085d9e2d 95 x = gd[0]/4*0.001;
WAT34 1:56dc085d9e2d 96 y = gd[1]/4*0.001;
WAT34 1:56dc085d9e2d 97 z = gd[2]/4*0.001;
WAT34 2:b8e04b6dc9ab 98 //各PIDの目標値を設定。
WAT34 2:b8e04b6dc9ab 99 tilt.setSetPoint(1.7);
WAT34 2:b8e04b6dc9ab 100 direct.setSetPoint(1.7);
WAT34 1:56dc085d9e2d 101 tilt.setProcessValue(atan2(x,y));
WAT34 1:56dc085d9e2d 102 tc = tilt.compute();
WAT34 2:b8e04b6dc9ab 103 //方位センサの値でPID制御
WAT34 2:b8e04b6dc9ab 104 deg = mag.sample();
WAT34 2:b8e04b6dc9ab 105 degree = deg+540-da;
WAT34 2:b8e04b6dc9ab 106 degree %= 360;
WAT34 2:b8e04b6dc9ab 107 degree -= 180;
WAT34 2:b8e04b6dc9ab 108 direct.setProcessValue(degree);
WAT34 2:b8e04b6dc9ab 109 dc = direct.compute();
WAT34 2:b8e04b6dc9ab 110 motort.speed(tc);
WAT34 2:b8e04b6dc9ab 111 motord.speed(dc);
WAT34 2:b8e04b6dc9ab 112 printf("degree%d\n\r",degree);
WAT34 1:56dc085d9e2d 113
WAT34 1:56dc085d9e2d 114 //発射官制
WAT34 1:56dc085d9e2d 115 if (sw == 0 && c == 0) {
WAT34 1:56dc085d9e2d 116 sound();
WAT34 1:56dc085d9e2d 117 sho.attach(&shoot,3.0);
WAT34 1:56dc085d9e2d 118 c = 1;
WAT34 1:56dc085d9e2d 119 } else if(sw2 == 0 && c == 0) {
WAT34 1:56dc085d9e2d 120 sound();
WAT34 1:56dc085d9e2d 121 rev.attach(&ret,3.0);
WAT34 1:56dc085d9e2d 122 c = 1;
WAT34 1:56dc085d9e2d 123 } else {
WAT34 0:7ec77e777058 124 air = 0;
WAT34 0:7ec77e777058 125 air2 = 0;
WAT34 0:7ec77e777058 126 }
WAT34 0:7ec77e777058 127 }
WAT34 0:7ec77e777058 128 }