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@3:80c921093b4d, 2015-07-08 (annotated)

Committer:
WAT34
Date:
Wed Jul 08 11:05:16 2015 +0000
Revision:
3:80c921093b4d
Parent:
2:b8e04b6dc9ab
Child:
4:bd8e35d546d7
lpc1114ver; ;

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