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@4:bd8e35d546d7, 2015-07-11 (annotated)

Committer:
WAT34
Date:
Sat Jul 11 05:18:44 2015 +0000
Revision:
4:bd8e35d546d7
Parent:
3:80c921093b4d 
nakata

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