Alice Furuya / Mbed 2 deprecated test

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:   mbed

main.cpp@0:baf1c4d4d305, 2020-01-09 (annotated)

Committer:
pumpkinhead
Date:
Thu Jan 09 04:40:33 2020 +0000
Revision:
0:baf1c4d4d305
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pumpkinhead 0:baf1c4d4d305 1 #include "mbed.h"
pumpkinhead 0:baf1c4d4d305 2
pumpkinhead 0:baf1c4d4d305 3 // 定数
pumpkinhead 0:baf1c4d4d305 4 #define PI 3.14159265358979F
pumpkinhead 0:baf1c4d4d305 5
pumpkinhead 0:baf1c4d4d305 6 #define STATE_ALL_BLACK 0
pumpkinhead 0:baf1c4d4d305 7 #define STATE_ALL_WHITE 1
pumpkinhead 0:baf1c4d4d305 8 #define STATE_LEFT 2
pumpkinhead 0:baf1c4d4d305 9 #define STATE_RIGHT 3
pumpkinhead 0:baf1c4d4d305 10 #define STATE_B_W_B 4
pumpkinhead 0:baf1c4d4d305 11
pumpkinhead 0:baf1c4d4d305 12 #define LR_STRAIGHT 0
pumpkinhead 0:baf1c4d4d305 13 #define LR_LEFT 1
pumpkinhead 0:baf1c4d4d305 14 #define LR_RIGHT 2
pumpkinhead 0:baf1c4d4d305 15
pumpkinhead 0:baf1c4d4d305 16 // パラメータ
pumpkinhead 0:baf1c4d4d305 17 #define INTERVAL 0.001F
pumpkinhead 0:baf1c4d4d305 18 #define BASE_SPEED 3400.0F
pumpkinhead 0:baf1c4d4d305 19 #define MIN_DUTY_RATIO 0.00F
pumpkinhead 0:baf1c4d4d305 20 #define MAX_DUTY_RATIO 0.85F
pumpkinhead 0:baf1c4d4d305 21
pumpkinhead 0:baf1c4d4d305 22 #define v_Kp 0.000040F // 比例ゲイン　（速度）
pumpkinhead 0:baf1c4d4d305 23 #define BASE_DUTY 0.43F // 定数ゲイン
pumpkinhead 0:baf1c4d4d305 24
pumpkinhead 0:baf1c4d4d305 25 #define r_Kp 2200.0F // 比例ゲイン　（転回）
pumpkinhead 0:baf1c4d4d305 26 #define r_Ki 3200.0F // 積分ゲイン　
pumpkinhead 0:baf1c4d4d305 27 #define r_Kd 300.0F // 微分ゲイン
pumpkinhead 0:baf1c4d4d305 28
pumpkinhead 0:baf1c4d4d305 29 #define LR_ADJ_RATIO 0.04F // 転回時　内側の減回転数と外側の増回転数の比（1.0:全部外側/0.0:全部内側）
pumpkinhead 0:baf1c4d4d305 30
pumpkinhead 0:baf1c4d4d305 31 //left side motor
pumpkinhead 0:baf1c4d4d305 32 PwmOut L_motor_out_back(D0);
pumpkinhead 0:baf1c4d4d305 33 PwmOut L_motor_out_forw(D3);
pumpkinhead 0:baf1c4d4d305 34
pumpkinhead 0:baf1c4d4d305 35 //right side motor
pumpkinhead 0:baf1c4d4d305 36 PwmOut R_motor_out_back(D5);
pumpkinhead 0:baf1c4d4d305 37 PwmOut R_motor_out_forw(D6);
pumpkinhead 0:baf1c4d4d305 38
pumpkinhead 0:baf1c4d4d305 39 //mode
pumpkinhead 0:baf1c4d4d305 40 DigitalOut mode(D1);
pumpkinhead 0:baf1c4d4d305 41
pumpkinhead 0:baf1c4d4d305 42 //read interupter voltage
pumpkinhead 0:baf1c4d4d305 43 InterruptIn L_motor_interupter_in(D12);
pumpkinhead 0:baf1c4d4d305 44 InterruptIn R_motor_interupter_in(D11);
pumpkinhead 0:baf1c4d4d305 45
pumpkinhead 0:baf1c4d4d305 46 //read reflector voltage
pumpkinhead 0:baf1c4d4d305 47 AnalogIn L_reflector_in(A2);
pumpkinhead 0:baf1c4d4d305 48 AnalogIn M_reflector_in(A1);
pumpkinhead 0:baf1c4d4d305 49 AnalogIn R_reflector_in(A0);
pumpkinhead 0:baf1c4d4d305 50
pumpkinhead 0:baf1c4d4d305 51 //電源確認用
pumpkinhead 0:baf1c4d4d305 52 DigitalOut led_out_power(D9);
pumpkinhead 0:baf1c4d4d305 53 //measure 200mm
pumpkinhead 0:baf1c4d4d305 54 DigitalOut led_out_mileage(D10);
pumpkinhead 0:baf1c4d4d305 55
pumpkinhead 0:baf1c4d4d305 56
pumpkinhead 0:baf1c4d4d305 57 int L_pulse_per_interval; //1ループのパルス数
pumpkinhead 0:baf1c4d4d305 58 int R_pulse_per_interval;
pumpkinhead 0:baf1c4d4d305 59 int L_pulse_per_interval_prev; //1ループのパルス数　前回
pumpkinhead 0:baf1c4d4d305 60 int R_pulse_per_interval_prev;
pumpkinhead 0:baf1c4d4d305 61 int L_pulse_total; //パルスカウンター（トータル）
pumpkinhead 0:baf1c4d4d305 62 int R_pulse_total;
pumpkinhead 0:baf1c4d4d305 63 int L_pulse_total_prev; //パルスカウンター（トータル）前回
pumpkinhead 0:baf1c4d4d305 64 int R_pulse_total_prev;
pumpkinhead 0:baf1c4d4d305 65 int k_counter1; //オドメトリ用のカウンター
pumpkinhead 0:baf1c4d4d305 66 int k_counter2;
pumpkinhead 0:baf1c4d4d305 67
pumpkinhead 0:baf1c4d4d305 68 //======================================================================
pumpkinhead 0:baf1c4d4d305 69 // 回転数カウウント用イベントハンドラー
pumpkinhead 0:baf1c4d4d305 70 //======================================================================
pumpkinhead 0:baf1c4d4d305 71 void L_motor_event_handler(void)
pumpkinhead 0:baf1c4d4d305 72 {
pumpkinhead 0:baf1c4d4d305 73 L_pulse_total++;
pumpkinhead 0:baf1c4d4d305 74 k_counter1++;
pumpkinhead 0:baf1c4d4d305 75 }
pumpkinhead 0:baf1c4d4d305 76
pumpkinhead 0:baf1c4d4d305 77 void R_motor_event_handler(void)
pumpkinhead 0:baf1c4d4d305 78 {
pumpkinhead 0:baf1c4d4d305 79 R_pulse_total++;
pumpkinhead 0:baf1c4d4d305 80 k_counter2++;
pumpkinhead 0:baf1c4d4d305 81 }
pumpkinhead 0:baf1c4d4d305 82 //======================================================================
pumpkinhead 0:baf1c4d4d305 83 // 転回のための左右速度調整幅決定関数
pumpkinhead 0:baf1c4d4d305 84 //======================================================================
pumpkinhead 0:baf1c4d4d305 85 void get_speed_adjust( float *L_speed_adjust, float *R_speed_adjust )
pumpkinhead 0:baf1c4d4d305 86 {
pumpkinhead 0:baf1c4d4d305 87 static float deviation[2]; // 進行方向偏差
pumpkinhead 0:baf1c4d4d305 88 static float integral; // 進行方向偏差の積分
pumpkinhead 0:baf1c4d4d305 89
pumpkinhead 0:baf1c4d4d305 90 static int state[3]; // リフレクターの常態 (STATE_ALL_BLACK/STATE_ALL_WHITE/STATE_B_W_B/STATE_LEFT/STATE_RIGHT)
pumpkinhead 0:baf1c4d4d305 91 static int LR_dir[3]; // 進行方向 (LR_STRAIGHT/LR_LEFT/LR_RIGHT)
pumpkinhead 0:baf1c4d4d305 92
pumpkinhead 0:baf1c4d4d305 93 // リフレクターの状態取得
pumpkinhead 0:baf1c4d4d305 94 //unit conversion (to V)
pumpkinhead 0:baf1c4d4d305 95 float L_ref_V = 3.3F * L_reflector_in;
pumpkinhead 0:baf1c4d4d305 96 float M_ref_V = 3.3F * M_reflector_in;
pumpkinhead 0:baf1c4d4d305 97 float R_ref_V = 3.3F * R_reflector_in;
pumpkinhead 0:baf1c4d4d305 98
pumpkinhead 0:baf1c4d4d305 99 state[2] = state[1];
pumpkinhead 0:baf1c4d4d305 100 state[1] = state[0];
pumpkinhead 0:baf1c4d4d305 101
pumpkinhead 0:baf1c4d4d305 102 state[0] =
pumpkinhead 0:baf1c4d4d305 103 ( L_ref_V > 2.5F && M_ref_V > 2.5F && R_ref_V > 2.5F)? STATE_ALL_BLACK:
pumpkinhead 0:baf1c4d4d305 104 ( L_ref_V < 1.3F && M_ref_V < 1.3F && R_ref_V < 1.3F)? STATE_ALL_WHITE:
pumpkinhead 0:baf1c4d4d305 105 ( L_ref_V > 2.0F && M_ref_V < 1.3F && R_ref_V > 2.0F)? STATE_B_W_B:
pumpkinhead 0:baf1c4d4d305 106 ( L_ref_V > R_ref_V)? STATE_LEFT : STATE_RIGHT;
pumpkinhead 0:baf1c4d4d305 107
pumpkinhead 0:baf1c4d4d305 108
pumpkinhead 0:baf1c4d4d305 109 // 進行方向の決定
pumpkinhead 0:baf1c4d4d305 110 if (LR_dir[0] != LR_dir[1]) {LR_dir[2] = LR_dir[1];}
pumpkinhead 0:baf1c4d4d305 111
pumpkinhead 0:baf1c4d4d305 112 LR_dir[1] = LR_dir[0];
pumpkinhead 0:baf1c4d4d305 113
pumpkinhead 0:baf1c4d4d305 114 LR_dir[0] =
pumpkinhead 0:baf1c4d4d305 115 (state[0] == STATE_ALL_BLACK)? // 全黒　直進
pumpkinhead 0:baf1c4d4d305 116 ((state[1] == STATE_ALL_BLACK)? LR_STRAIGHT : LR_dir[1]) : // 最初の全黒はノイズの可能性があるので無視して２回目から直進
pumpkinhead 0:baf1c4d4d305 117 (state[0] == STATE_B_W_B)? LR_dir[1] : // 真ん中白・両端黒はノイズを拾っている可能性があるので無視
pumpkinhead 0:baf1c4d4d305 118 (state[0] == STATE_ALL_WHITE)? // 全白　旋回継続
pumpkinhead 0:baf1c4d4d305 119 ((LR_dir[1] == LR_STRAIGHT)? LR_dir[2] : LR_dir[1]) : // 全白の前全黒の場合はその前の旋回方向を選択
pumpkinhead 0:baf1c4d4d305 120 (state[0] == STATE_LEFT)?
pumpkinhead 0:baf1c4d4d305 121 ((state[1] == STATE_ALL_WHITE && LR_dir[1] == LR_RIGHT)? LR_RIGHT : LR_LEFT) : // 全白旋回中の突然の方向転換はライン以外の黒を拾っている可能性があるので無視
pumpkinhead 0:baf1c4d4d305 122 ((state[1] == STATE_ALL_WHITE && LR_dir[1] == LR_LEFT)? LR_LEFT : LR_RIGHT);
pumpkinhead 0:baf1c4d4d305 123
pumpkinhead 0:baf1c4d4d305 124 if (LR_dir[0] == LR_LEFT && L_ref_V < M_ref_V) { // 乖離方向と反対のリフレクタが真ん中より明るい場合、反対のリフレクタの明るさを真ん中の明るさと同じと仮定
pumpkinhead 0:baf1c4d4d305 125 L_ref_V = M_ref_V;
pumpkinhead 0:baf1c4d4d305 126 }
pumpkinhead 0:baf1c4d4d305 127 if (LR_dir[0] == LR_RIGHT && R_ref_V < M_ref_V) {
pumpkinhead 0:baf1c4d4d305 128 R_ref_V = M_ref_V;
pumpkinhead 0:baf1c4d4d305 129 }
pumpkinhead 0:baf1c4d4d305 130
pumpkinhead 0:baf1c4d4d305 131 // 進行方向偏差
pumpkinhead 0:baf1c4d4d305 132 deviation[0] = // 偏差はリフレクターの明るさ度合いの合計
pumpkinhead 0:baf1c4d4d305 133 ((L_ref_V > 2.5F)? 0 : 2.5F - L_ref_V)
pumpkinhead 0:baf1c4d4d305 134 + ((M_ref_V > 2.5F)? 0 : 2.5F - M_ref_V)
pumpkinhead 0:baf1c4d4d305 135 + ((R_ref_V > 2.5F)? 0 : 2.5F - R_ref_V);
pumpkinhead 0:baf1c4d4d305 136
pumpkinhead 0:baf1c4d4d305 137 // 進行方向偏差の積分
pumpkinhead 0:baf1c4d4d305 138 integral =
pumpkinhead 0:baf1c4d4d305 139 (LR_dir[0] == LR_STRAIGHT)? 0.0F: // 直進時リセット
pumpkinhead 0:baf1c4d4d305 140 (LR_dir[0] == LR_dir[1])? integral + deviation[0] * INTERVAL : 0.0F; // 方向転換時リセット
pumpkinhead 0:baf1c4d4d305 141
pumpkinhead 0:baf1c4d4d305 142 integral = (integral > 0.9F)? 0.9F : integral; // 積分要素の上限
pumpkinhead 0:baf1c4d4d305 143
pumpkinhead 0:baf1c4d4d305 144 // 操作幅　比例成分
pumpkinhead 0:baf1c4d4d305 145 float adjust = deviation[0] * r_Kp;
pumpkinhead 0:baf1c4d4d305 146
pumpkinhead 0:baf1c4d4d305 147 // 操作幅に積分成分追加
pumpkinhead 0:baf1c4d4d305 148 adjust += integral * r_Ki;
pumpkinhead 0:baf1c4d4d305 149
pumpkinhead 0:baf1c4d4d305 150 // 操作幅に微分成分追加
pumpkinhead 0:baf1c4d4d305 151 adjust += (deviation[0] - deviation[1]) * r_Kd / INTERVAL;
pumpkinhead 0:baf1c4d4d305 152
pumpkinhead 0:baf1c4d4d305 153 // 操作を左右の車輪に分配
pumpkinhead 0:baf1c4d4d305 154 *L_speed_adjust =
pumpkinhead 0:baf1c4d4d305 155 (LR_dir[0] == LR_STRAIGHT)? 0.0F :
pumpkinhead 0:baf1c4d4d305 156 (LR_dir[0] == LR_LEFT)? (LR_ADJ_RATIO -1.0F)*adjust : adjust * LR_ADJ_RATIO;
pumpkinhead 0:baf1c4d4d305 157
pumpkinhead 0:baf1c4d4d305 158 *R_speed_adjust =
pumpkinhead 0:baf1c4d4d305 159 (LR_dir[0] == LR_STRAIGHT)? 0.0F :
pumpkinhead 0:baf1c4d4d305 160 (LR_dir[0] == LR_RIGHT)? (LR_ADJ_RATIO -1.0F)*adjust : adjust * LR_ADJ_RATIO;
pumpkinhead 0:baf1c4d4d305 161
pumpkinhead 0:baf1c4d4d305 162
pumpkinhead 0:baf1c4d4d305 163 deviation[1] = deviation[0];
pumpkinhead 0:baf1c4d4d305 164 }
pumpkinhead 0:baf1c4d4d305 165 //======================================================================
pumpkinhead 0:baf1c4d4d305 166 // メイン
pumpkinhead 0:baf1c4d4d305 167 //======================================================================
pumpkinhead 0:baf1c4d4d305 168 int main()
pumpkinhead 0:baf1c4d4d305 169 {
pumpkinhead 0:baf1c4d4d305 170
pumpkinhead 0:baf1c4d4d305 171 float L_speed_adjust;
pumpkinhead 0:baf1c4d4d305 172 float R_speed_adjust;
pumpkinhead 0:baf1c4d4d305 173
pumpkinhead 0:baf1c4d4d305 174 float L_duty_ratio;
pumpkinhead 0:baf1c4d4d305 175 float R_duty_ratio;
pumpkinhead 0:baf1c4d4d305 176
pumpkinhead 0:baf1c4d4d305 177 float L_target; //left target level 左の目標値 (pulse per loop)
pumpkinhead 0:baf1c4d4d305 178 float R_target; //right target level 右の目標値 (pulse per loop)
pumpkinhead 0:baf1c4d4d305 179
pumpkinhead 0:baf1c4d4d305 180 int oddeven; //LED点滅の偶数回奇数回を判定
pumpkinhead 0:baf1c4d4d305 181
pumpkinhead 0:baf1c4d4d305 182
pumpkinhead 0:baf1c4d4d305 183 float k = 0.0; //距離計測
pumpkinhead 0:baf1c4d4d305 184 float is_stop; // 静止状態の時間計測
pumpkinhead 0:baf1c4d4d305 185
pumpkinhead 0:baf1c4d4d305 186 mode = 0;
pumpkinhead 0:baf1c4d4d305 187
pumpkinhead 0:baf1c4d4d305 188 // motor output period
pumpkinhead 0:baf1c4d4d305 189 L_motor_out_back.period_us(32);
pumpkinhead 0:baf1c4d4d305 190 L_motor_out_forw.period_us(32);
pumpkinhead 0:baf1c4d4d305 191 R_motor_out_back.period_us(32);
pumpkinhead 0:baf1c4d4d305 192 R_motor_out_forw.period_us(32);
pumpkinhead 0:baf1c4d4d305 193
pumpkinhead 0:baf1c4d4d305 194 L_motor_out_back.write(0.0);
pumpkinhead 0:baf1c4d4d305 195 L_motor_out_forw.write(0.0);
pumpkinhead 0:baf1c4d4d305 196 R_motor_out_back.write(0.0);
pumpkinhead 0:baf1c4d4d305 197 R_motor_out_forw.write(0.0);
pumpkinhead 0:baf1c4d4d305 198
pumpkinhead 0:baf1c4d4d305 199 led_out_power = 1;
pumpkinhead 0:baf1c4d4d305 200
pumpkinhead 0:baf1c4d4d305 201 wait(4.0);
pumpkinhead 0:baf1c4d4d305 202
pumpkinhead 0:baf1c4d4d305 203 led_out_mileage = 1;
pumpkinhead 0:baf1c4d4d305 204
pumpkinhead 0:baf1c4d4d305 205 L_pulse_total = 0;
pumpkinhead 0:baf1c4d4d305 206 R_pulse_total = 0;
pumpkinhead 0:baf1c4d4d305 207 L_pulse_total_prev = 0;
pumpkinhead 0:baf1c4d4d305 208 R_pulse_total_prev = 0;
pumpkinhead 0:baf1c4d4d305 209 L_pulse_per_interval = 0;
pumpkinhead 0:baf1c4d4d305 210 R_pulse_per_interval = 0;
pumpkinhead 0:baf1c4d4d305 211
pumpkinhead 0:baf1c4d4d305 212 oddeven = 1;
pumpkinhead 0:baf1c4d4d305 213
pumpkinhead 0:baf1c4d4d305 214 L_duty_ratio = 0;
pumpkinhead 0:baf1c4d4d305 215 R_duty_ratio = 0;
pumpkinhead 0:baf1c4d4d305 216
pumpkinhead 0:baf1c4d4d305 217 L_motor_interupter_in.rise(&L_motor_event_handler);
pumpkinhead 0:baf1c4d4d305 218 L_motor_interupter_in.fall(&L_motor_event_handler);
pumpkinhead 0:baf1c4d4d305 219 R_motor_interupter_in.rise(&R_motor_event_handler);
pumpkinhead 0:baf1c4d4d305 220 R_motor_interupter_in.fall(&R_motor_event_handler);
pumpkinhead 0:baf1c4d4d305 221
pumpkinhead 0:baf1c4d4d305 222 while(1) {
pumpkinhead 0:baf1c4d4d305 223
pumpkinhead 0:baf1c4d4d305 224 get_speed_adjust( &L_speed_adjust, &R_speed_adjust );
pumpkinhead 0:baf1c4d4d305 225
pumpkinhead 0:baf1c4d4d305 226 L_target = BASE_SPEED + L_speed_adjust;
pumpkinhead 0:baf1c4d4d305 227 R_target = BASE_SPEED + R_speed_adjust;
pumpkinhead 0:baf1c4d4d305 228
pumpkinhead 0:baf1c4d4d305 229 L_pulse_per_interval_prev = L_pulse_per_interval;
pumpkinhead 0:baf1c4d4d305 230 R_pulse_per_interval_prev = R_pulse_per_interval;
pumpkinhead 0:baf1c4d4d305 231
pumpkinhead 0:baf1c4d4d305 232 L_pulse_per_interval = L_pulse_total - L_pulse_total_prev;
pumpkinhead 0:baf1c4d4d305 233 R_pulse_per_interval = R_pulse_total - R_pulse_total_prev;
pumpkinhead 0:baf1c4d4d305 234
pumpkinhead 0:baf1c4d4d305 235 L_pulse_total_prev = L_pulse_total_prev + L_pulse_per_interval;
pumpkinhead 0:baf1c4d4d305 236 R_pulse_total_prev = R_pulse_total_prev + R_pulse_per_interval;
pumpkinhead 0:baf1c4d4d305 237
pumpkinhead 0:baf1c4d4d305 238 L_duty_ratio = BASE_DUTY + L_target * v_Kp
pumpkinhead 0:baf1c4d4d305 239 + (L_target - L_pulse_per_interval/INTERVAL) * v_Kp;
pumpkinhead 0:baf1c4d4d305 240
pumpkinhead 0:baf1c4d4d305 241 R_duty_ratio = BASE_DUTY + R_target * v_Kp
pumpkinhead 0:baf1c4d4d305 242 + (R_target - R_pulse_per_interval/INTERVAL) * v_Kp;
pumpkinhead 0:baf1c4d4d305 243 // 上限チェック
pumpkinhead 0:baf1c4d4d305 244 L_duty_ratio = (L_duty_ratio > MAX_DUTY_RATIO)? MAX_DUTY_RATIO : L_duty_ratio;
pumpkinhead 0:baf1c4d4d305 245 R_duty_ratio = (R_duty_ratio > MAX_DUTY_RATIO)? MAX_DUTY_RATIO : R_duty_ratio;
pumpkinhead 0:baf1c4d4d305 246 // 下限チェック
pumpkinhead 0:baf1c4d4d305 247 L_duty_ratio = (L_duty_ratio < MIN_DUTY_RATIO)? MIN_DUTY_RATIO : L_duty_ratio;
pumpkinhead 0:baf1c4d4d305 248 R_duty_ratio = (R_duty_ratio < MIN_DUTY_RATIO)? MIN_DUTY_RATIO : R_duty_ratio;
pumpkinhead 0:baf1c4d4d305 249
pumpkinhead 0:baf1c4d4d305 250 is_stop = (L_pulse_per_interval == 0 && R_pulse_per_interval == 0)?
pumpkinhead 0:baf1c4d4d305 251 is_stop + INTERVAL : 0.0F;
pumpkinhead 0:baf1c4d4d305 252
pumpkinhead 0:baf1c4d4d305 253 // 静止状態が3秒続いたら、高 duty 比を入力
pumpkinhead 0:baf1c4d4d305 254 if (is_stop > 3.0F) {
pumpkinhead 0:baf1c4d4d305 255 L_motor_out_forw.write(0.87);
pumpkinhead 0:baf1c4d4d305 256 R_motor_out_forw.write(0.87);
pumpkinhead 0:baf1c4d4d305 257 } else {
pumpkinhead 0:baf1c4d4d305 258 L_motor_out_forw.write(L_duty_ratio);
pumpkinhead 0:baf1c4d4d305 259 R_motor_out_forw.write(R_duty_ratio);
pumpkinhead 0:baf1c4d4d305 260 }
pumpkinhead 0:baf1c4d4d305 261
pumpkinhead 0:baf1c4d4d305 262
pumpkinhead 0:baf1c4d4d305 263
pumpkinhead 0:baf1c4d4d305 264 k = (((k_counter1 + k_counter2) / (24.0F * 38.2F)) * 55.0F * PI) / 2.0F;
pumpkinhead 0:baf1c4d4d305 265
pumpkinhead 0:baf1c4d4d305 266 if(k >= 200.0F) {
pumpkinhead 0:baf1c4d4d305 267 if((oddeven % 2) == 1) { //200mmおきにLEDを点滅。奇数回目のループだったらLEDを消す
pumpkinhead 0:baf1c4d4d305 268 led_out_mileage = 0;
pumpkinhead 0:baf1c4d4d305 269 } else {
pumpkinhead 0:baf1c4d4d305 270 led_out_mileage = 1;
pumpkinhead 0:baf1c4d4d305 271 }
pumpkinhead 0:baf1c4d4d305 272 oddeven++;
pumpkinhead 0:baf1c4d4d305 273 k_counter1 = 0;
pumpkinhead 0:baf1c4d4d305 274 k_counter2 = 0;
pumpkinhead 0:baf1c4d4d305 275 }
pumpkinhead 0:baf1c4d4d305 276 wait(INTERVAL);
pumpkinhead 0:baf1c4d4d305 277 }
pumpkinhead 0:baf1c4d4d305 278 }
pumpkinhead 0:baf1c4d4d305 279