12_han_meiji / Mbed 2 deprecated 10_if_else_control

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:5e29061237cd, 2020-01-14 (annotated)

Committer:
komachiangel72
Date:
Tue Jan 14 04:31:57 2020 +0000
Revision:
0:5e29061237cd
k

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komachiangel72 0:5e29061237cd 1 #include "mbed.h"
komachiangel72 0:5e29061237cd 2
komachiangel72 0:5e29061237cd 3 int mode_debug = 1; //printf()表示
komachiangel72 0:5e29061237cd 4
komachiangel72 0:5e29061237cd 5
komachiangel72 0:5e29061237cd 6
komachiangel72 0:5e29061237cd 7 //走行用プログラム
komachiangel72 0:5e29061237cd 8 //モータードライバー＋回転数計測プログラム
komachiangel72 0:5e29061237cd 9 //走るためのプログラム+道が白か黒かを判定するプログラム
komachiangel72 0:5e29061237cd 10 //A入力に右のモーター、B入力に左のモーター
komachiangel72 0:5e29061237cd 11 //58mm径タイヤの一周は約182.21mm
komachiangel72 0:5e29061237cd 12
komachiangel72 0:5e29061237cd 13 InterruptIn enc_R(D11);//フォトインタラプタ右
komachiangel72 0:5e29061237cd 14 InterruptIn enc_L(D12);//フォトインタラプタ左
komachiangel72 0:5e29061237cd 15
komachiangel72 0:5e29061237cd 16 AnalogIn sensor_R(A0);//進行方向右のフォトリフレクタセンサー
komachiangel72 0:5e29061237cd 17 AnalogIn sensor_L(A2);//進行方向左のセンサー
komachiangel72 0:5e29061237cd 18 AnalogIn sensor_C(A1);//中央配置のセンサー
komachiangel72 0:5e29061237cd 19 int R_on_off =0;//左右の白黒
komachiangel72 0:5e29061237cd 20 int L_on_off =0;
komachiangel72 0:5e29061237cd 21 int C_on_off =0;
komachiangel72 0:5e29061237cd 22 int pre_R_on_off =0;//1つ前の過程の、センサで決めた白黒（白なら0、黒なら1）
komachiangel72 0:5e29061237cd 23 int pre_L_on_off =0;
komachiangel72 0:5e29061237cd 24 int pre_C_on_off =1;//スタート時、中央センサーのみ黒の上
komachiangel72 0:5e29061237cd 25
komachiangel72 0:5e29061237cd 26 int frame_counter = 0; //時間変化関数
komachiangel72 0:5e29061237cd 27
komachiangel72 0:5e29061237cd 28 DigitalOut myled(A5);//200mmごとに光らせるLED用
komachiangel72 0:5e29061237cd 29 DigitalOut power_on_led(A6);
komachiangel72 0:5e29061237cd 30
komachiangel72 0:5e29061237cd 31
komachiangel72 0:5e29061237cd 32 PwmOut AIN1(D6);//右モーター
komachiangel72 0:5e29061237cd 33 PwmOut AIN2(D5);
komachiangel72 0:5e29061237cd 34 PwmOut BIN1(D9);//左モーター
komachiangel72 0:5e29061237cd 35 PwmOut BIN2(D10);//D9とD10は回路上で逆にしないと、なぜか回転方向が逆転
komachiangel72 0:5e29061237cd 36
komachiangel72 0:5e29061237cd 37 //デューティー比関連の変数、定数
komachiangel72 0:5e29061237cd 38
komachiangel72 0:5e29061237cd 39 const float R_duty_basic = 0.22f;//基本のデューティー比//0.25
komachiangel72 0:5e29061237cd 40 const float L_duty_basic = 0.23f;//基本のデューティー比//0.25
komachiangel72 0:5e29061237cd 41 const float roll_duty_small = 0.19f;//右左折時に作用 白黒黒//0.25
komachiangel72 0:5e29061237cd 42 const float roll_duty_large = 0.21f;//右左折時に作用 白黒黒//0.30
komachiangel72 0:5e29061237cd 43 const float medium_roll_duty_small = 0.19f;//中右左折時に作用 白白黒//0.25
komachiangel72 0:5e29061237cd 44 const float medium_roll_duty_large = 0.25f;//中右左折時に作用 白白黒//0.35
komachiangel72 0:5e29061237cd 45 const float sharp_roll_duty_forward = 0.20f;//急カーブ時に作用 超信地旋回 白白白//0.25
komachiangel72 0:5e29061237cd 46 const float sharp_roll_duty_back = 0.20f;//急カーブ時に作用 超信地旋回 白白白//0.25
komachiangel72 0:5e29061237cd 47 //const float duty_ignition = 0.20f;//急カーブ修了時にスピードアップ
komachiangel72 0:5e29061237cd 48
komachiangel72 0:5e29061237cd 49 //int re_ignition_counter = 0;//下の格納
komachiangel72 0:5e29061237cd 50 //int re_ignition_limit = 2;//スピードアップのカウンター(回数＊0.01s)
komachiangel72 0:5e29061237cd 51
komachiangel72 0:5e29061237cd 52 //フォトリフレクタセンサ関連の定数
komachiangel72 0:5e29061237cd 53
komachiangel72 0:5e29061237cd 54 const float L_black = 2.3;//左センサの黒値
komachiangel72 0:5e29061237cd 55 const float L_white = 0.3;//白値
komachiangel72 0:5e29061237cd 56 const float R_black = 0.8;//右センサの黒値
komachiangel72 0:5e29061237cd 57 const float R_white = 0.2;//
komachiangel72 0:5e29061237cd 58 const float C_black = 1.2;//中心センサの黒値
komachiangel72 0:5e29061237cd 59 const float C_white = 0.2;//
komachiangel72 0:5e29061237cd 60
komachiangel72 0:5e29061237cd 61
komachiangel72 0:5e29061237cd 62 //float L_target = (L_black + L_white) / 2;
komachiangel72 0:5e29061237cd 63 //float R_target = (R_black + R_white) / 2;
komachiangel72 0:5e29061237cd 64 //float C_target = (C_black + C_white) / 2;
komachiangel72 0:5e29061237cd 65
komachiangel72 0:5e29061237cd 66 float L_target = 1.0;
komachiangel72 0:5e29061237cd 67 float R_target = 0.6;
komachiangel72 0:5e29061237cd 68 float C_target = 0.6;
komachiangel72 0:5e29061237cd 69
komachiangel72 0:5e29061237cd 70 //回転数関連の変数、定数
komachiangel72 0:5e29061237cd 71 float R_rot=0;//右タイヤの１秒間あたり回転数
komachiangel72 0:5e29061237cd 72 float L_rot=0;
komachiangel72 0:5e29061237cd 73 float pre_R_rot=0;//1つ前の過程の右モーターの累計回転数
komachiangel72 0:5e29061237cd 74 float pre_L_rot=0;
komachiangel72 0:5e29061237cd 75 float R_target_rot=0;//１秒間あたりの右目標タイヤ回転数
komachiangel72 0:5e29061237cd 76 float L_target_rot=0;//１秒間あたりの左目標タイヤ回転数
komachiangel72 0:5e29061237cd 77 float counter_R =0;//右モーターの累計回転数
komachiangel72 0:5e29061237cd 78 float pre_counter_R = 0;//1つ前の過程の右のモーターの累計回転数
komachiangel72 0:5e29061237cd 79 float counter_L =0;
komachiangel72 0:5e29061237cd 80 float pre_counter_L = 0;
komachiangel72 0:5e29061237cd 81
komachiangel72 0:5e29061237cd 82 //距離、時間関係の変数、定数
komachiangel72 0:5e29061237cd 83 float distance_per_frame = 0.0f;//距離の初期値
komachiangel72 0:5e29061237cd 84 float total_distance = 0.0f;//距離のトータル
komachiangel72 0:5e29061237cd 85 float delta = 0.01;//計測、判断時間の周期
komachiangel72 0:5e29061237cd 86
komachiangel72 0:5e29061237cd 87 //モーター回転数計測
komachiangel72 0:5e29061237cd 88 void event_handler_R(void){//右モーター回転数計測
komachiangel72 0:5e29061237cd 89 counter_R++;
komachiangel72 0:5e29061237cd 90 }
komachiangel72 0:5e29061237cd 91
komachiangel72 0:5e29061237cd 92 void event_handler_L(void){//左モーター
komachiangel72 0:5e29061237cd 93 counter_L++;
komachiangel72 0:5e29061237cd 94 }
komachiangel72 0:5e29061237cd 95
komachiangel72 0:5e29061237cd 96
komachiangel72 0:5e29061237cd 97 //ブレーキ
komachiangel72 0:5e29061237cd 98 void motorStop(PwmOut IN1,PwmOut IN2) {
komachiangel72 0:5e29061237cd 99 IN1 = 1;
komachiangel72 0:5e29061237cd 100 IN2 = 1;
komachiangel72 0:5e29061237cd 101 }
komachiangel72 0:5e29061237cd 102
komachiangel72 0:5e29061237cd 103 //正転
komachiangel72 0:5e29061237cd 104
komachiangel72 0:5e29061237cd 105 void motorForward(PwmOut IN1,PwmOut IN2,float duty) {
komachiangel72 0:5e29061237cd 106 IN1 = duty;
komachiangel72 0:5e29061237cd 107 IN2 = 0;
komachiangel72 0:5e29061237cd 108 }
komachiangel72 0:5e29061237cd 109
komachiangel72 0:5e29061237cd 110 //逆転
komachiangel72 0:5e29061237cd 111 void motorReverse(PwmOut IN1,PwmOut IN2,float duty) {
komachiangel72 0:5e29061237cd 112 IN1 = 0;
komachiangel72 0:5e29061237cd 113 IN2 = duty;
komachiangel72 0:5e29061237cd 114 }
komachiangel72 0:5e29061237cd 115 //空転
komachiangel72 0:5e29061237cd 116 void motorIdling(PwmOut IN1,PwmOut IN2) {
komachiangel72 0:5e29061237cd 117 IN1 = 0;
komachiangel72 0:5e29061237cd 118 IN2 = 0;
komachiangel72 0:5e29061237cd 119 }
komachiangel72 0:5e29061237cd 120
komachiangel72 0:5e29061237cd 121 //停止
komachiangel72 0:5e29061237cd 122 void machine_Stop() {
komachiangel72 0:5e29061237cd 123 motorStop(AIN1,AIN2);
komachiangel72 0:5e29061237cd 124 motorStop(BIN1,BIN2);
komachiangel72 0:5e29061237cd 125 }
komachiangel72 0:5e29061237cd 126
komachiangel72 0:5e29061237cd 127 //前進
komachiangel72 0:5e29061237cd 128 void machine_Forward(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 129 motorReverse(AIN1,AIN2,duty_R);//逆転のほうが制御しやすいので前進時は逆転を使用
komachiangel72 0:5e29061237cd 130 motorReverse(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 131 }
komachiangel72 0:5e29061237cd 132
komachiangel72 0:5e29061237cd 133 //後退
komachiangel72 0:5e29061237cd 134 void machine_Back(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 135 machine_Stop();
komachiangel72 0:5e29061237cd 136 wait(2);
komachiangel72 0:5e29061237cd 137 motorForward(AIN1,AIN2,duty_R);
komachiangel72 0:5e29061237cd 138 motorForward(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 139 }
komachiangel72 0:5e29061237cd 140 /*
komachiangel72 0:5e29061237cd 141 //右カーブ
komachiangel72 0:5e29061237cd 142 void Right_rotation(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 143
komachiangel72 0:5e29061237cd 144 motorReverse(AIN1,AIN2,duty_R);
komachiangel72 0:5e29061237cd 145 motorReverse(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 146 }
komachiangel72 0:5e29061237cd 147
komachiangel72 0:5e29061237cd 148 //左カーブ
komachiangel72 0:5e29061237cd 149 void Left_rotation(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 150
komachiangel72 0:5e29061237cd 151 motorReverse(AIN1,AIN2,duty_R);
komachiangel72 0:5e29061237cd 152 motorReverse(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 153 }
komachiangel72 0:5e29061237cd 154 */
komachiangel72 0:5e29061237cd 155
komachiangel72 0:5e29061237cd 156
komachiangel72 0:5e29061237cd 157 //右カーブ(超信地旋回
komachiangel72 0:5e29061237cd 158 void Right_high_rotation(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 159
komachiangel72 0:5e29061237cd 160 motorForward(AIN1,AIN2,duty_R);
komachiangel72 0:5e29061237cd 161 motorReverse(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 162 }
komachiangel72 0:5e29061237cd 163
komachiangel72 0:5e29061237cd 164 //左カーブ(超信地旋回
komachiangel72 0:5e29061237cd 165 void Left_high_rotation(float duty_R,float duty_L) {
komachiangel72 0:5e29061237cd 166
komachiangel72 0:5e29061237cd 167 motorReverse(AIN1,AIN2,duty_R);
komachiangel72 0:5e29061237cd 168 motorForward(BIN1,BIN2,duty_L);
komachiangel72 0:5e29061237cd 169 }
komachiangel72 0:5e29061237cd 170
komachiangel72 0:5e29061237cd 171 enum branch_mode{//制御の状態記号
komachiangel72 0:5e29061237cd 172 STRAIGHT =0,
komachiangel72 0:5e29061237cd 173 RIGHT,
komachiangel72 0:5e29061237cd 174 LEFT,
komachiangel72 0:5e29061237cd 175 MEDIUM_R,
komachiangel72 0:5e29061237cd 176 MEDIUM_L,
komachiangel72 0:5e29061237cd 177 SHARP_R,
komachiangel72 0:5e29061237cd 178 SHARP_L,
komachiangel72 0:5e29061237cd 179 REVERSE
komachiangel72 0:5e29061237cd 180 };
komachiangel72 0:5e29061237cd 181
komachiangel72 0:5e29061237cd 182 int mode;//展開中の制御状態記号の格納
komachiangel72 0:5e29061237cd 183 int pre_mode = mode;//一つ前の状態記号の格納:線を外れた際に引用
komachiangel72 0:5e29061237cd 184
komachiangel72 0:5e29061237cd 185
komachiangel72 0:5e29061237cd 186 enum restart_ignition{//減速後の復帰のための操作に使用する信号
komachiangel72 0:5e29061237cd 187 no=0,
komachiangel72 0:5e29061237cd 188 caution,
komachiangel72 0:5e29061237cd 189 ready,
komachiangel72 0:5e29061237cd 190 yes
komachiangel72 0:5e29061237cd 191 };
komachiangel72 0:5e29061237cd 192
komachiangel72 0:5e29061237cd 193 //int re_ignition;//上restart_ignitionの格納
komachiangel72 0:5e29061237cd 194
komachiangel72 0:5e29061237cd 195 /*
komachiangel72 0:5e29061237cd 196 restart_ignitionについて
komachiangel72 0:5e29061237cd 197
komachiangel72 0:5e29061237cd 198 [re_ignition = no]では、通常の走行状態
komachiangel72 0:5e29061237cd 199 [re_ignition = ready]は急カーブ中の状態
komachiangel72 0:5e29061237cd 200 この状態で中央のセンサーが黒を感知すると[re_ignition = yes]となり、
komachiangel72 0:5e29061237cd 201 一時的にduty比を上げて、走行させる。その後[re_ignition = no]に戻る。
komachiangel72 0:5e29061237cd 202 */
komachiangel72 0:5e29061237cd 203
komachiangel72 0:5e29061237cd 204 //センサーの色判定と走行モード決め
komachiangel72 0:5e29061237cd 205 void Check_reflecter(AnalogIn sensor_R, AnalogIn sensor_L,AnalogIn sensor_C){//リフレクタの黒白判定＆移動方向の決定
komachiangel72 0:5e29061237cd 206 if(mode_debug==1){
komachiangel72 0:5e29061237cd 207 printf("sensor_Right:L432[%.3f]>\n",sensor_R*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 208 printf("sensor_Left:L432[%.3f]>\n",sensor_L*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 209 printf("sensor_Center:L432[%.3f]>\n\n",sensor_C*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 210 }
komachiangel72 0:5e29061237cd 211
komachiangel72 0:5e29061237cd 212 if(sensor_R*3.3F>R_target)
komachiangel72 0:5e29061237cd 213 {
komachiangel72 0:5e29061237cd 214 R_on_off=1;
komachiangel72 0:5e29061237cd 215 }else{
komachiangel72 0:5e29061237cd 216 R_on_off=0;
komachiangel72 0:5e29061237cd 217 }
komachiangel72 0:5e29061237cd 218 if(sensor_L*3.3F>L_target)
komachiangel72 0:5e29061237cd 219 {
komachiangel72 0:5e29061237cd 220 L_on_off=1;
komachiangel72 0:5e29061237cd 221 }else{
komachiangel72 0:5e29061237cd 222 L_on_off=0;
komachiangel72 0:5e29061237cd 223 }
komachiangel72 0:5e29061237cd 224 if(sensor_C*3.3F>C_target)
komachiangel72 0:5e29061237cd 225 {
komachiangel72 0:5e29061237cd 226 C_on_off=1;
komachiangel72 0:5e29061237cd 227 }else{
komachiangel72 0:5e29061237cd 228 C_on_off=0;
komachiangel72 0:5e29061237cd 229 }
komachiangel72 0:5e29061237cd 230
komachiangel72 0:5e29061237cd 231
komachiangel72 0:5e29061237cd 232 if(L_on_off==0 && C_on_off==0 && R_on_off==0)//線の外
komachiangel72 0:5e29061237cd 233 {
komachiangel72 0:5e29061237cd 234 if(mode == MEDIUM_R || mode == RIGHT || mode == SHARP_R)
komachiangel72 0:5e29061237cd 235 {
komachiangel72 0:5e29061237cd 236 mode = SHARP_R;
komachiangel72 0:5e29061237cd 237 Right_high_rotation(sharp_roll_duty_back,sharp_roll_duty_forward);
komachiangel72 0:5e29061237cd 238 ////re_ignition = ready;
komachiangel72 0:5e29061237cd 239 }else if(mode == MEDIUM_L || mode == LEFT || mode == SHARP_L){
komachiangel72 0:5e29061237cd 240 mode = SHARP_L;
komachiangel72 0:5e29061237cd 241 Right_high_rotation(sharp_roll_duty_forward,sharp_roll_duty_back);
komachiangel72 0:5e29061237cd 242 //re_ignition = ready;
komachiangel72 0:5e29061237cd 243 }
komachiangel72 0:5e29061237cd 244
komachiangel72 0:5e29061237cd 245 if(mode_debug==1){
komachiangel72 0:5e29061237cd 246 printf("I'm going out of line! Previous condition is %d \n",mode);
komachiangel72 0:5e29061237cd 247 }
komachiangel72 0:5e29061237cd 248
komachiangel72 0:5e29061237cd 249 }
komachiangel72 0:5e29061237cd 250 else if(L_on_off==1 && C_on_off==0 && R_on_off==0)//左折
komachiangel72 0:5e29061237cd 251 {
komachiangel72 0:5e29061237cd 252 mode = MEDIUM_L;
komachiangel72 0:5e29061237cd 253 machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
komachiangel72 0:5e29061237cd 254 //re_ignition = caution;
komachiangel72 0:5e29061237cd 255 if(mode_debug==1){
komachiangel72 0:5e29061237cd 256 printf("M_LEFT\n");
komachiangel72 0:5e29061237cd 257 }
komachiangel72 0:5e29061237cd 258 }
komachiangel72 0:5e29061237cd 259 else if(L_on_off==0 && C_on_off==0 && R_on_off==1)//右折
komachiangel72 0:5e29061237cd 260 {
komachiangel72 0:5e29061237cd 261 mode = MEDIUM_R;
komachiangel72 0:5e29061237cd 262 machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
komachiangel72 0:5e29061237cd 263 //re_ignition = caution;
komachiangel72 0:5e29061237cd 264 if(mode_debug==1){
komachiangel72 0:5e29061237cd 265 printf("M_RIGHT\n");
komachiangel72 0:5e29061237cd 266 }
komachiangel72 0:5e29061237cd 267 }
komachiangel72 0:5e29061237cd 268 else if(L_on_off==1 && C_on_off==0 && R_on_off==1)//黒白黒、エラー、back
komachiangel72 0:5e29061237cd 269 {
komachiangel72 0:5e29061237cd 270 //mode = REVERSE;
komachiangel72 0:5e29061237cd 271 //re_ignition = no;
komachiangel72 0:5e29061237cd 272 if(mode_debug==1){
komachiangel72 0:5e29061237cd 273 printf("REVERSE\n");
komachiangel72 0:5e29061237cd 274 }
komachiangel72 0:5e29061237cd 275 }
komachiangel72 0:5e29061237cd 276 else if(L_on_off==0 && C_on_off==1 && R_on_off==0)//前進
komachiangel72 0:5e29061237cd 277 {
komachiangel72 0:5e29061237cd 278 //re_ignition = no;
komachiangel72 0:5e29061237cd 279 machine_Forward(R_duty_basic,L_duty_basic);
komachiangel72 0:5e29061237cd 280 mode = STRAIGHT;
komachiangel72 0:5e29061237cd 281 if(mode_debug==1){
komachiangel72 0:5e29061237cd 282 printf("STRAIGHT\n");
komachiangel72 0:5e29061237cd 283 }
komachiangel72 0:5e29061237cd 284 }
komachiangel72 0:5e29061237cd 285 else if(L_on_off==1 && C_on_off==1 && R_on_off==0)//左折
komachiangel72 0:5e29061237cd 286 {
komachiangel72 0:5e29061237cd 287 //re_ignition =no;
komachiangel72 0:5e29061237cd 288 machine_Forward(roll_duty_large,roll_duty_small);
komachiangel72 0:5e29061237cd 289 mode = LEFT;
komachiangel72 0:5e29061237cd 290 if(mode_debug==1){
komachiangel72 0:5e29061237cd 291 printf("LEFT\n");
komachiangel72 0:5e29061237cd 292 }
komachiangel72 0:5e29061237cd 293 }
komachiangel72 0:5e29061237cd 294 else if(L_on_off==0 && C_on_off==1 && R_on_off==1)//右折
komachiangel72 0:5e29061237cd 295 {
komachiangel72 0:5e29061237cd 296 //re_ignition = no;
komachiangel72 0:5e29061237cd 297 machine_Forward(roll_duty_small,roll_duty_large);
komachiangel72 0:5e29061237cd 298 mode = RIGHT;
komachiangel72 0:5e29061237cd 299 if(mode_debug==1){
komachiangel72 0:5e29061237cd 300 printf("RIGHT\n");
komachiangel72 0:5e29061237cd 301 }
komachiangel72 0:5e29061237cd 302 }
komachiangel72 0:5e29061237cd 303 else if(L_on_off==1 && C_on_off==1 && R_on_off==1)//all black（空中or十字部)よって直進
komachiangel72 0:5e29061237cd 304 {
komachiangel72 0:5e29061237cd 305 //re_ignition = no;
komachiangel72 0:5e29061237cd 306 machine_Forward(R_duty_basic,L_duty_basic);
komachiangel72 0:5e29061237cd 307 mode = STRAIGHT;
komachiangel72 0:5e29061237cd 308 if(mode_debug==1){
komachiangel72 0:5e29061237cd 309 printf("STRAIGHT\n");
komachiangel72 0:5e29061237cd 310 }
komachiangel72 0:5e29061237cd 311 }
komachiangel72 0:5e29061237cd 312
komachiangel72 0:5e29061237cd 313 if(pre_C_on_off != C_on_off || pre_R_on_off != R_on_off || pre_L_on_off != L_on_off)//新地回転の前と後でre_ignitionを添加
komachiangel72 0:5e29061237cd 314 {
komachiangel72 0:5e29061237cd 315 /*if(re_ignition == caution || re_ignition ==ready)
komachiangel72 0:5e29061237cd 316 {
komachiangel72 0:5e29061237cd 317 re_ignition = yes;
komachiangel72 0:5e29061237cd 318 }*/
komachiangel72 0:5e29061237cd 319 if(mode_debug==1)
komachiangel72 0:5e29061237cd 320 {
komachiangel72 0:5e29061237cd 321 printf("Judgement of sensor L[%d] C[%d] R[%d]\n",L_on_off,C_on_off,R_on_off);//[1]なら黒判定
komachiangel72 0:5e29061237cd 322 }
komachiangel72 0:5e29061237cd 323 //frame_counter = 0;
komachiangel72 0:5e29061237cd 324 }
komachiangel72 0:5e29061237cd 325 else{
komachiangel72 0:5e29061237cd 326 //frame_counter++;
komachiangel72 0:5e29061237cd 327 }
komachiangel72 0:5e29061237cd 328 }
komachiangel72 0:5e29061237cd 329
komachiangel72 0:5e29061237cd 330
komachiangel72 0:5e29061237cd 331
komachiangel72 0:5e29061237cd 332 //距離計算関数
komachiangel72 0:5e29061237cd 333 void distance_calculate (void)
komachiangel72 0:5e29061237cd 334 {
komachiangel72 0:5e29061237cd 335 distance_per_frame = (R_rot+L_rot)/2.0f*182.12f;//右の回転数と左の回転数の平均に、一周の距離をかける
komachiangel72 0:5e29061237cd 336 total_distance += distance_per_frame;
komachiangel72 0:5e29061237cd 337 printf("Total Distance [%f] Distance per Frame [%f]\n",total_distance,distance_per_frame);
komachiangel72 0:5e29061237cd 338 printf("Left and right rotation speed per frame R[%f] L[%f]\n",R_rot,L_rot);
komachiangel72 0:5e29061237cd 339
komachiangel72 0:5e29061237cd 340
komachiangel72 0:5e29061237cd 341 }
komachiangel72 0:5e29061237cd 342
komachiangel72 0:5e29061237cd 343 //LED点灯プログラム
komachiangel72 0:5e29061237cd 344 void led_lightning(float total_distance)
komachiangel72 0:5e29061237cd 345 {
komachiangel72 0:5e29061237cd 346 int period_100mm =0;
komachiangel72 0:5e29061237cd 347 for (int i =0;i<500;i++)
komachiangel72 0:5e29061237cd 348 {
komachiangel72 0:5e29061237cd 349 if( total_distance >= (float)i*200 && total_distance < ((float)i+1)*200)
komachiangel72 0:5e29061237cd 350 {
komachiangel72 0:5e29061237cd 351 period_100mm = i;
komachiangel72 0:5e29061237cd 352 break;
komachiangel72 0:5e29061237cd 353 }
komachiangel72 0:5e29061237cd 354 }
komachiangel72 0:5e29061237cd 355 if(period_100mm%2 == 1)
komachiangel72 0:5e29061237cd 356 {
komachiangel72 0:5e29061237cd 357 myled = 1;
komachiangel72 0:5e29061237cd 358 printf("LED IS [ON]\n");
komachiangel72 0:5e29061237cd 359
komachiangel72 0:5e29061237cd 360 }else
komachiangel72 0:5e29061237cd 361 {
komachiangel72 0:5e29061237cd 362 myled = 0;
komachiangel72 0:5e29061237cd 363 printf("LED IS [OFF]\n\n\n");
komachiangel72 0:5e29061237cd 364 }
komachiangel72 0:5e29061237cd 365 }
komachiangel72 0:5e29061237cd 366
komachiangel72 0:5e29061237cd 367
komachiangel72 0:5e29061237cd 368 void memorize_privious_value(void){
komachiangel72 0:5e29061237cd 369 pre_mode = mode;
komachiangel72 0:5e29061237cd 370
komachiangel72 0:5e29061237cd 371 pre_C_on_off = C_on_off;
komachiangel72 0:5e29061237cd 372 pre_R_on_off = R_on_off;
komachiangel72 0:5e29061237cd 373 pre_L_on_off = L_on_off;
komachiangel72 0:5e29061237cd 374
komachiangel72 0:5e29061237cd 375 pre_counter_R = counter_R;
komachiangel72 0:5e29061237cd 376 pre_counter_L = counter_L;
komachiangel72 0:5e29061237cd 377 pre_R_rot=R_rot;
komachiangel72 0:5e29061237cd 378 pre_R_rot=R_rot;
komachiangel72 0:5e29061237cd 379 }
komachiangel72 0:5e29061237cd 380
komachiangel72 0:5e29061237cd 381 int main() {
komachiangel72 0:5e29061237cd 382 AIN1.period(0.001);
komachiangel72 0:5e29061237cd 383 AIN2.period(0.001);
komachiangel72 0:5e29061237cd 384 BIN1.period(0.001);
komachiangel72 0:5e29061237cd 385 BIN2.period(0.001);
komachiangel72 0:5e29061237cd 386
komachiangel72 0:5e29061237cd 387 power_on_led=1;
komachiangel72 0:5e29061237cd 388 machine_Stop();
komachiangel72 0:5e29061237cd 389 mode = STRAIGHT;
komachiangel72 0:5e29061237cd 390 //re_ignition=no;
komachiangel72 0:5e29061237cd 391
komachiangel72 0:5e29061237cd 392 while(1) {
komachiangel72 0:5e29061237cd 393 // wait(delta);
komachiangel72 0:5e29061237cd 394 Check_reflecter(sensor_R,sensor_L,sensor_C);
komachiangel72 0:5e29061237cd 395
komachiangel72 0:5e29061237cd 396 distance_calculate();
komachiangel72 0:5e29061237cd 397
komachiangel72 0:5e29061237cd 398 memorize_privious_value();
komachiangel72 0:5e29061237cd 399 led_lightning(total_distance);
komachiangel72 0:5e29061237cd 400 }
komachiangel72 0:5e29061237cd 401 }