NITICRobotClub 2018Team-B / Mbed 2 deprecated Lets_move_Seki2018_null

自動投射のみです。（手動用にチューニングしました）

Dependencies:   HCSR04 PID QEI mbed

Fork of Lets_move_Seki2018_ver2 by INCTRC Information Sharing Test

main.cpp@2:c32991ba628f, 2018-08-18 (annotated)

Committer:
yuron
Date:
Sat Aug 18 04:59:28 2018 +0000
Revision:
2:c32991ba628f
Parent:
1:62b321f6c9c3
Child:
3:1223cab367d9
????MD??????????????(2018/08/18??)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuron 0:f73c1b076ae4 1 /* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
yuron 2:c32991ba628f 2 /* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
yuron 0:f73c1b076ae4 3 /* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
yuron 0:f73c1b076ae4 4 /* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
yuron 0:f73c1b076ae4 5 /* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
yuron 0:f73c1b076ae4 6 /* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
yuron 0:f73c1b076ae4 7 /* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 8 /* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 9 /* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 10 /* 2018/6/9追記 */
yuron 0:f73c1b076ae4 11 /* ついに回転数の！PI制御を実現できました。 */
yuron 0:f73c1b076ae4 12 /* タイマ割込みで角速度を取得してみようと思います。 */
yuron 0:f73c1b076ae4 13
yuron 0:f73c1b076ae4 14 #include "mbed.h"
yuron 0:f73c1b076ae4 15 #include "math.h"
yuron 0:f73c1b076ae4 16 #include "QEI.h"
yuron 0:f73c1b076ae4 17 #include "PID.h"
yuron 0:f73c1b076ae4 18 #define PI 3.14159265359
yuron 0:f73c1b076ae4 19
yuron 0:f73c1b076ae4 20 PID controller(0.7, 0.7, 0.0, 0.001);
yuron 2:c32991ba628f 21 PID RF(0.5, 0.4, 0.0, 0.001);
yuron 0:f73c1b076ae4 22 PID RB(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 23 PID LF(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 24 PID LB(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 25 I2C i2c(PB_3, PB_10); //SDA, SCL
yuron 1:62b321f6c9c3 26 Serial pc(USBTX, USBRX);
yuron 2:c32991ba628f 27 Serial photo(D8, D2);
yuron 0:f73c1b076ae4 28 DigitalOut emergency(PA_13);
yuron 1:62b321f6c9c3 29 DigitalOut myled(D13);
yuron 0:f73c1b076ae4 30
yuron 0:f73c1b076ae4 31 /* 以下446基板用 */
yuron 0:f73c1b076ae4 32 QEI wheel_LB(PA_1, PA_0, NC, 624);
yuron 0:f73c1b076ae4 33 QEI wheel_RB(PB_6, PB_7, NC, 624);
yuron 0:f73c1b076ae4 34 QEI wheel_LF(PB_4, PB_5, NC, 624);
yuron 0:f73c1b076ae4 35 QEI wheel_RF(PC_8, PC_9, NC, 624);
yuron 0:f73c1b076ae4 36
yuron 0:f73c1b076ae4 37 Ticker get_RPS;
yuron 0:f73c1b076ae4 38 Ticker print_RPS;
yuron 1:62b321f6c9c3 39 Timer timer;
yuron 0:f73c1b076ae4 40
yuron 0:f73c1b076ae4 41 int rps_RF;
yuron 0:f73c1b076ae4 42 int rps_RB;
yuron 0:f73c1b076ae4 43 int rps_LF;
yuron 0:f73c1b076ae4 44 int rps_LB;
yuron 0:f73c1b076ae4 45
yuron 0:f73c1b076ae4 46 int rpm_RF;
yuron 0:f73c1b076ae4 47 int rpm_RB;
yuron 0:f73c1b076ae4 48 int rpm_LF;
yuron 0:f73c1b076ae4 49 int rpm_LB;
yuron 0:f73c1b076ae4 50
yuron 0:f73c1b076ae4 51 int pulse_RF;
yuron 0:f73c1b076ae4 52 int pulse_RB;
yuron 0:f73c1b076ae4 53 int pulse_LF;
yuron 0:f73c1b076ae4 54 int pulse_LB;
yuron 0:f73c1b076ae4 55
yuron 0:f73c1b076ae4 56 int counter_RF;
yuron 0:f73c1b076ae4 57 int counter_RB;
yuron 0:f73c1b076ae4 58 int counter_LF;
yuron 0:f73c1b076ae4 59 int counter_LB;
yuron 0:f73c1b076ae4 60
yuron 0:f73c1b076ae4 61 double a_v; /* 角速度 */
yuron 0:f73c1b076ae4 62 double n_v; /* 速度(秒速) */
yuron 0:f73c1b076ae4 63 double h_v; /* 速度(時速) */
yuron 0:f73c1b076ae4 64 double n_a; /* 加速度 */
yuron 0:f73c1b076ae4 65
yuron 0:f73c1b076ae4 66 char send_data[1];
yuron 0:f73c1b076ae4 67 char true_send_data[1];
yuron 0:f73c1b076ae4 68
yuron 0:f73c1b076ae4 69 char RF_data[1];
yuron 0:f73c1b076ae4 70 char RB_data[1];
yuron 0:f73c1b076ae4 71 char LF_data[1];
yuron 0:f73c1b076ae4 72 char LB_data[1];
yuron 0:f73c1b076ae4 73 char true_RF_data[1];
yuron 0:f73c1b076ae4 74 char true_RB_data[1];
yuron 0:f73c1b076ae4 75 char true_LF_data[1];
yuron 0:f73c1b076ae4 76 char true_LB_data[1];
yuron 0:f73c1b076ae4 77
yuron 0:f73c1b076ae4 78 /* ロリコン処理関数 */
yuron 0:f73c1b076ae4 79 void flip();
yuron 0:f73c1b076ae4 80 /* RPS表示関数 */
yuron 0:f73c1b076ae4 81 void flip2();
yuron 0:f73c1b076ae4 82
yuron 1:62b321f6c9c3 83 /*
yuron 1:62b321f6c9c3 84 void ice(int address, int data)
yuron 1:62b321f6c9c3 85 {
yuron 1:62b321f6c9c3 86 //周波数: 100kHz
yuron 1:62b321f6c9c3 87 //i2c.frequency(100000);
yuron 1:62b321f6c9c3 88
yuron 1:62b321f6c9c3 89 // スタートコンディション出力
yuron 1:62b321f6c9c3 90 i2c.start();
yuron 1:62b321f6c9c3 91 wait_us(20);
yuron 1:62b321f6c9c3 92
yuron 1:62b321f6c9c3 93 // アドレスの書き込み
yuron 1:62b321f6c9c3 94 i2c.write(address);
yuron 1:62b321f6c9c3 95 wait_us(20);
yuron 1:62b321f6c9c3 96
yuron 1:62b321f6c9c3 97 // データの書き込み
yuron 1:62b321f6c9c3 98 i2c.write(data);
yuron 1:62b321f6c9c3 99 wait_us(20);
yuron 1:62b321f6c9c3 100
yuron 1:62b321f6c9c3 101 // ストップコンディション出力
yuron 1:62b321f6c9c3 102 i2c.stop();
yuron 1:62b321f6c9c3 103 wait_us(90);
yuron 1:62b321f6c9c3 104 }
yuron 1:62b321f6c9c3 105 */
yuron 1:62b321f6c9c3 106
yuron 0:f73c1b076ae4 107 void flip(){
yuron 0:f73c1b076ae4 108
yuron 0:f73c1b076ae4 109 pulse_RF = wheel_RF.getPulses();
yuron 0:f73c1b076ae4 110 pulse_RB = wheel_RB.getPulses();
yuron 0:f73c1b076ae4 111 pulse_LF = wheel_LF.getPulses();
yuron 0:f73c1b076ae4 112 pulse_LB = wheel_LB.getPulses();
yuron 0:f73c1b076ae4 113
yuron 0:f73c1b076ae4 114 /* *rps変換 */
yuron 0:f73c1b076ae4 115 /*10ms*100 = 1s
yuron 0:f73c1b076ae4 116 counter_RB = pulse_RB * 100;
yuron 0:f73c1b076ae4 117 counter_LB = pulse_LB * 100;
yuron 0:f73c1b076ae4 118 */
yuron 0:f73c1b076ae4 119
yuron 0:f73c1b076ae4 120 //40ms*25 = 1s
yuron 0:f73c1b076ae4 121 counter_RF = pulse_RF * 25;
yuron 0:f73c1b076ae4 122 counter_RB = pulse_RB * 25;
yuron 0:f73c1b076ae4 123 counter_LF = pulse_LF * 25;
yuron 0:f73c1b076ae4 124 counter_LB = pulse_LB * 25;
yuron 0:f73c1b076ae4 125
yuron 0:f73c1b076ae4 126 /*
yuron 0:f73c1b076ae4 127 //100ms*10 = 1s
yuron 0:f73c1b076ae4 128 counter_RB = pulse_RB * 10;
yuron 0:f73c1b076ae4 129 counter_LB = pulse_LB * 10;
yuron 0:f73c1b076ae4 130 */
yuron 0:f73c1b076ae4 131
yuron 0:f73c1b076ae4 132 /* /分解能 */
yuron 0:f73c1b076ae4 133 rps_RF = counter_RF / 100;
yuron 0:f73c1b076ae4 134 rps_RB = counter_RB / 100;
yuron 0:f73c1b076ae4 135 rps_LF = counter_LF / 100;
yuron 0:f73c1b076ae4 136 rps_LB = counter_LB / 100;
yuron 0:f73c1b076ae4 137
yuron 0:f73c1b076ae4 138 rpm_RF = pulse_RF * 25 * 60 / 100;
yuron 0:f73c1b076ae4 139 rpm_RB = pulse_RB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 140 rpm_LF = pulse_LF * 25 * 60 / 100;
yuron 0:f73c1b076ae4 141 rpm_LB = pulse_LB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 142
yuron 0:f73c1b076ae4 143 /*
yuron 0:f73c1b076ae4 144 rps[0] = counter_RB / 100;
yuron 0:f73c1b076ae4 145 rps[1] = counter_LB / 100;
yuron 0:f73c1b076ae4 146
yuron 0:f73c1b076ae4 147 rpm[0] = pulse_RB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 148 rpm[1] = pulse_LB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 149 */
yuron 0:f73c1b076ae4 150
yuron 0:f73c1b076ae4 151 /* RPMから角速度へ変換 */
yuron 0:f73c1b076ae4 152 a_v = rpm_LB * PI / 30;
yuron 0:f73c1b076ae4 153
yuron 0:f73c1b076ae4 154 /* RPMから速度(秒速)へ変換 */
yuron 0:f73c1b076ae4 155 /* タイヤ半径を0.05[m]とする */
yuron 0:f73c1b076ae4 156 n_v = 0.05 * 2 * PI * rpm_LB / 60;
yuron 0:f73c1b076ae4 157
yuron 0:f73c1b076ae4 158 /* 速度(秒速)から速度(時速)へ変換 */
yuron 0:f73c1b076ae4 159 h_v = n_v * 3600 / 1000;
yuron 0:f73c1b076ae4 160
yuron 0:f73c1b076ae4 161 //n_a = n_v /
yuron 0:f73c1b076ae4 162
yuron 0:f73c1b076ae4 163 /*
yuron 0:f73c1b076ae4 164 if(rpm[1] < 200){
yuron 0:f73c1b076ae4 165 send_data[0]--;
yuron 0:f73c1b076ae4 166 }
yuron 0:f73c1b076ae4 167 else if(rpm[1] > 250){
yuron 0:f73c1b076ae4 168 send_data[1]++;
yuron 0:f73c1b076ae4 169 }
yuron 0:f73c1b076ae4 170 */
yuron 0:f73c1b076ae4 171
yuron 0:f73c1b076ae4 172 //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
yuron 2:c32991ba628f 173 pc.printf("%d\n", rpm_LF);
yuron 0:f73c1b076ae4 174 //pc.printf("%lf\n", n_v);
yuron 0:f73c1b076ae4 175 //pc.printf("%lf\n", h_v);
yuron 0:f73c1b076ae4 176 //pc.printf("rpm: %d n_v: %lf\n", rpm[1], n_v);
yuron 2:c32991ba628f 177 //pc.printf("%lf\n", timer.read());
yuron 0:f73c1b076ae4 178
yuron 0:f73c1b076ae4 179 wheel_RF.reset();
yuron 0:f73c1b076ae4 180 wheel_RB.reset();
yuron 0:f73c1b076ae4 181 wheel_LF.reset();
yuron 0:f73c1b076ae4 182 wheel_LB.reset();
yuron 0:f73c1b076ae4 183 }
yuron 0:f73c1b076ae4 184
yuron 0:f73c1b076ae4 185 void flip2()
yuron 0:f73c1b076ae4 186 {
yuron 0:f73c1b076ae4 187 //pc.printf("RPS_RB: %d RPS_LB: %d\n", abs(rps[0]), abs(rps[1]));
yuron 0:f73c1b076ae4 188 //pc.printf("RPM_RB: %d RPM_LB: %d\n", abs(rpm[0]), abs(rpm[1]));
yuron 1:62b321f6c9c3 189 //pc.printf("%d\n", rpm_RF);
yuron 0:f73c1b076ae4 190 //pc.printf("%lf\n", a_v);
yuron 0:f73c1b076ae4 191 //pc.printf("rpm: %d a_v: %lf\n", rpm[1], a_v);
yuron 0:f73c1b076ae4 192 }
yuron 0:f73c1b076ae4 193
yuron 2:c32991ba628f 194 int front_PID(int setpoint)
yuron 0:f73c1b076ae4 195 {
yuron 0:f73c1b076ae4 196 // センサ出力値の最小、最大
yuron 2:c32991ba628f 197 RF.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 198 RB.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 199 LF.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 200 LB.setInputLimits(-2000, 2000);
yuron 0:f73c1b076ae4 201
yuron 0:f73c1b076ae4 202 // 制御量の最小、最大
yuron 2:c32991ba628f 203 RF.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 204 RB.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 205 LF.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 206 LB.setOutputLimits(0x0C, 0x7C);
yuron 0:f73c1b076ae4 207
yuron 0:f73c1b076ae4 208 // よくわからんやつ
yuron 0:f73c1b076ae4 209 RF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 210 RB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 211 LF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 212 LB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 213
yuron 1:62b321f6c9c3 214 //RF: -
yuron 1:62b321f6c9c3 215 //RB: -
yuron 1:62b321f6c9c3 216 //LF: +
yuron 1:62b321f6c9c3 217 //LB: +
yuron 1:62b321f6c9c3 218
yuron 0:f73c1b076ae4 219 // 目標値
yuron 2:c32991ba628f 220 RF.setSetPoint(setpoint);
yuron 2:c32991ba628f 221 RB.setSetPoint(setpoint);
yuron 2:c32991ba628f 222 LF.setSetPoint(setpoint);
yuron 2:c32991ba628f 223 LB.setSetPoint(setpoint);
yuron 0:f73c1b076ae4 224
yuron 0:f73c1b076ae4 225 // センサ出力
yuron 1:62b321f6c9c3 226 RF.setProcessValue(rpm_RF);
yuron 1:62b321f6c9c3 227 RB.setProcessValue(rpm_RB);
yuron 1:62b321f6c9c3 228 LF.setProcessValue(rpm_LF);
yuron 1:62b321f6c9c3 229 LB.setProcessValue(rpm_LB);
yuron 0:f73c1b076ae4 230
yuron 0:f73c1b076ae4 231 // 制御量(計算結果)
yuron 0:f73c1b076ae4 232 RF_data[0] = RF.compute();
yuron 0:f73c1b076ae4 233 RB_data[0] = RB.compute();
yuron 0:f73c1b076ae4 234 LF_data[0] = LF.compute();
yuron 0:f73c1b076ae4 235 LB_data[0] = LB.compute();
yuron 0:f73c1b076ae4 236
yuron 0:f73c1b076ae4 237 // 制御量をPWM値に変換
yuron 2:c32991ba628f 238 true_RF_data[0] = 0x7D - RF_data[0];
yuron 2:c32991ba628f 239 true_RB_data[0] = 0x7D - RB_data[0];
yuron 2:c32991ba628f 240 true_LF_data[0] = 0x7D - LF_data[0];
yuron 2:c32991ba628f 241 true_LB_data[0] = 0x7D - LB_data[0];
yuron 2:c32991ba628f 242
yuron 2:c32991ba628f 243 return 0;
yuron 0:f73c1b076ae4 244 }
yuron 0:f73c1b076ae4 245
yuron 2:c32991ba628f 246 int back_PID(int setpoint)
yuron 1:62b321f6c9c3 247 {
yuron 1:62b321f6c9c3 248 // センサ出力値の最小、最大
yuron 2:c32991ba628f 249 RF.setInputLimits(0, 2000);
yuron 2:c32991ba628f 250 RB.setInputLimits(0, 2000);
yuron 2:c32991ba628f 251 LF.setInputLimits(0, 2000);
yuron 2:c32991ba628f 252 LB.setInputLimits(0, 2000);
yuron 1:62b321f6c9c3 253
yuron 1:62b321f6c9c3 254 // 制御量の最小、最大
yuron 2:c32991ba628f 255 RF.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 256 RB.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 257 LF.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 258 LB.setOutputLimits(0x84, 0xFF);
yuron 1:62b321f6c9c3 259
yuron 1:62b321f6c9c3 260 // よくわからんやつ
yuron 1:62b321f6c9c3 261 RF.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 262 RB.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 263 LF.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 264 LB.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 265
yuron 1:62b321f6c9c3 266 // 目標値
yuron 2:c32991ba628f 267 RF.setSetPoint(setpoint);
yuron 2:c32991ba628f 268 RB.setSetPoint(setpoint);
yuron 2:c32991ba628f 269 LF.setSetPoint(setpoint);
yuron 2:c32991ba628f 270 LB.setSetPoint(setpoint);
yuron 1:62b321f6c9c3 271
yuron 1:62b321f6c9c3 272 // センサ出力
yuron 2:c32991ba628f 273 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 2:c32991ba628f 274 RF.setProcessValue(abs(rpm_RF));
yuron 2:c32991ba628f 275 RB.setProcessValue(abs(rpm_RB));
yuron 2:c32991ba628f 276 LF.setProcessValue(abs(rpm_LF));
yuron 2:c32991ba628f 277 LB.setProcessValue(abs(rpm_LB));
yuron 1:62b321f6c9c3 278
yuron 1:62b321f6c9c3 279 // 制御量(計算結果)
yuron 1:62b321f6c9c3 280 RF_data[0] = RF.compute();
yuron 1:62b321f6c9c3 281 RB_data[0] = RB.compute();
yuron 1:62b321f6c9c3 282 LF_data[0] = LF.compute();
yuron 1:62b321f6c9c3 283 LB_data[0] = LB.compute();
yuron 1:62b321f6c9c3 284
yuron 2:c32991ba628f 285 return 0;
yuron 1:62b321f6c9c3 286 }
yuron 1:62b321f6c9c3 287
yuron 0:f73c1b076ae4 288 int main(void)
yuron 0:f73c1b076ae4 289 {
yuron 0:f73c1b076ae4 290 emergency = 0;
yuron 2:c32991ba628f 291 send_data[0] = 0x80;
yuron 0:f73c1b076ae4 292 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 293 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 294 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 295 i2c.write(0xA6, send_data, 1);
yuron 0:f73c1b076ae4 296 wait(0.1);
yuron 0:f73c1b076ae4 297
yuron 0:f73c1b076ae4 298 /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
yuron 0:f73c1b076ae4 299 get_RPS.attach_us(&flip, 40000);
yuron 0:f73c1b076ae4 300 //get_RPS.attach_us(&flip, 100000);
yuron 0:f73c1b076ae4 301
yuron 0:f73c1b076ae4 302 //RPS表示(0.1sサイクル)
yuron 0:f73c1b076ae4 303 //print_RPS.attach(&flip2, 0.1);
yuron 0:f73c1b076ae4 304
yuron 0:f73c1b076ae4 305 while(1)
yuron 0:f73c1b076ae4 306 {
yuron 2:c32991ba628f 307 /*
yuron 2:c32991ba628f 308 //後進PID確認
yuron 2:c32991ba628f 309 back_PID(200);
yuron 2:c32991ba628f 310 i2c.write(0x20, LF_data, 1, false);
yuron 2:c32991ba628f 311 wait_us(20);
yuron 2:c32991ba628f 312 */
yuron 0:f73c1b076ae4 313
yuron 2:c32991ba628f 314 timer.start();
yuron 1:62b321f6c9c3 315 while(timer.read() <= 5.0f) {
yuron 2:c32991ba628f 316 myled = 1;
yuron 2:c32991ba628f 317 front_PID(200);
yuron 2:c32991ba628f 318 i2c.write(0x20, true_LF_data, 1, false);
yuron 1:62b321f6c9c3 319 wait_us(20);
yuron 1:62b321f6c9c3 320 }
yuron 1:62b321f6c9c3 321 LF.reset();
yuron 2:c32991ba628f 322 while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
yuron 2:c32991ba628f 323 myled = 0;
yuron 2:c32991ba628f 324 true_LF_data[0] = 0x80;
yuron 2:c32991ba628f 325 i2c.write(0x20, true_LF_data, 1, false);
yuron 2:c32991ba628f 326 wait_us(20);
yuron 2:c32991ba628f 327 }
yuron 2:c32991ba628f 328 while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
yuron 2:c32991ba628f 329 myled = 1;
yuron 2:c32991ba628f 330 back_PID(200);
yuron 2:c32991ba628f 331 i2c.write(0x20, LF_data, 1, false);
yuron 2:c32991ba628f 332 wait_us(20);
yuron 2:c32991ba628f 333 }
yuron 2:c32991ba628f 334 LF.reset();
yuron 2:c32991ba628f 335 while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
yuron 2:c32991ba628f 336 myled = 0;
yuron 2:c32991ba628f 337 true_LF_data[0] = 0x80;
yuron 2:c32991ba628f 338 i2c.write(0x20, true_LF_data, 1, false);
yuron 2:c32991ba628f 339 wait_us(20);
yuron 1:62b321f6c9c3 340 }
yuron 1:62b321f6c9c3 341 timer.reset();
yuron 0:f73c1b076ae4 342
yuron 0:f73c1b076ae4 343 /*
yuron 0:f73c1b076ae4 344 // センサ出力値の最小、最大
yuron 0:f73c1b076ae4 345 RF.setInputLimits(0, 1100);
yuron 0:f73c1b076ae4 346 RB.setInputLimits(0, 1100);
yuron 0:f73c1b076ae4 347 LF.setInputLimits(0, 1100);
yuron 0:f73c1b076ae4 348 LB.setInputLimits(0, 1100);
yuron 0:f73c1b076ae4 349
yuron 0:f73c1b076ae4 350 // 制御量の最小、最大
yuron 0:f73c1b076ae4 351 RF.setOutputLimits(0x01, 0x37);
yuron 0:f73c1b076ae4 352 RB.setOutputLimits(0x01, 0x37);
yuron 0:f73c1b076ae4 353 LF.setOutputLimits(0x01, 0x37);
yuron 0:f73c1b076ae4 354 LB.setOutputLimits(0x01, 0x37);
yuron 0:f73c1b076ae4 355
yuron 0:f73c1b076ae4 356 // よくわからんやつ
yuron 0:f73c1b076ae4 357 RF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 358 RB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 359 LF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 360 LB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 361
yuron 0:f73c1b076ae4 362 // 目標値
yuron 0:f73c1b076ae4 363 RF.setSetPoint(400);
yuron 0:f73c1b076ae4 364 RB.setSetPoint(400);
yuron 0:f73c1b076ae4 365 LF.setSetPoint(400);
yuron 0:f73c1b076ae4 366 LB.setSetPoint(400);
yuron 0:f73c1b076ae4 367
yuron 0:f73c1b076ae4 368 // センサ出力
yuron 0:f73c1b076ae4 369 RF.setProcessValue(abs(rpm_RF));
yuron 0:f73c1b076ae4 370 RB.setProcessValue(abs(rpm_RB));
yuron 0:f73c1b076ae4 371 LF.setProcessValue(abs(rpm_LF));
yuron 0:f73c1b076ae4 372 LB.setProcessValue(abs(rpm_LB));
yuron 0:f73c1b076ae4 373
yuron 0:f73c1b076ae4 374 // 制御量(計算結果)
yuron 0:f73c1b076ae4 375 RF_data[0] = RF.compute();
yuron 0:f73c1b076ae4 376 RB_data[0] = RB.compute();
yuron 0:f73c1b076ae4 377 LF_data[0] = LF.compute();
yuron 0:f73c1b076ae4 378 LB_data[0] = LB.compute();
yuron 0:f73c1b076ae4 379
yuron 0:f73c1b076ae4 380 // 制御量をPWM値に変換
yuron 0:f73c1b076ae4 381 true_RF_data[0] = 0x38 - RF_data[0];
yuron 0:f73c1b076ae4 382 true_RB_data[0] = 0x38 - RB_data[0];
yuron 0:f73c1b076ae4 383 true_LF_data[0] = 0x38 - LF_data[0];
yuron 0:f73c1b076ae4 384 true_LB_data[0] = 0x38 - LB_data[0];
yuron 0:f73c1b076ae4 385 */
yuron 0:f73c1b076ae4 386
yuron 0:f73c1b076ae4 387 /*
yuron 2:c32991ba628f 388 //どんどん加速(逆転)
yuron 2:c32991ba628f 389 for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){
yuron 0:f73c1b076ae4 390 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 391 //ice(0xA2, send_data);
yuron 2:c32991ba628f 392
yuron 0:f73c1b076ae4 393 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 394 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 395 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 396 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 397
yuron 2:c32991ba628f 398 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 399 wait(0.1);
yuron 0:f73c1b076ae4 400 }
yuron 2:c32991ba628f 401 //だんだん減速(逆転)
yuron 2:c32991ba628f 402 for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){
yuron 0:f73c1b076ae4 403 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 404 //ice(0xA2, send_data);
yuron 2:c32991ba628f 405
yuron 0:f73c1b076ae4 406 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 407 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 408 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 409 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 410
yuron 2:c32991ba628f 411 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 412 wait(0.1);
yuron 0:f73c1b076ae4 413 }
yuron 2:c32991ba628f 414 send_data[0] = 0x80;
yuron 2:c32991ba628f 415 i2c.write(0x20, send_data, 1);
yuron 2:c32991ba628f 416 wait(5);
yuron 0:f73c1b076ae4 417 */
yuron 0:f73c1b076ae4 418 }
yuron 0:f73c1b076ae4 419 }