Yuhi Takaku / Mbed 2 deprecated omni_wheel

Bチーム自動ロボットの足回りプログラム(2018/07/16時点) PIDかけて多方向に進もうとしたらMDが燃えたプログラム（´・ω・‘）

Dependencies:   PID QEI mbed

Fork of omni_wheel by INCTRC Information Sharing Test

main.cpp@1:62b321f6c9c3, 2018-07-15 (annotated)

Committer:
yuron
Date:
Sun Jul 15 23:56:24 2018 +0000
Revision:
1:62b321f6c9c3
Parent:
0:f73c1b076ae4 
B??????????????????(2018/07/16??) PID??????????????MD????????????????; ;

Who changed what in which revision?

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