12_han_meiji / Mbed 2 deprecated 10_2_ifelse_lighter

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@1:3b12960a1b5e, 2020-01-14 (annotated)

Committer:
maenoshin
Date:
Tue Jan 14 11:38:30 2020 +0000
Revision:
1:3b12960a1b5e
Parent:
0:5e29061237cd 
a

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
maenoshin 1:3b12960a1b5e 45 const float sharp_roll_duty_forward = 0.25f;//急カーブ時に作用 超信地旋回 白白白//0.25
maenoshin 1:3b12960a1b5e 46 const float sharp_roll_duty_back = 0.25f;//急カーブ時に作用 超信地旋回 白白白//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 //フォトリフレクタセンサ関連の定数
maenoshin 1:3b12960a1b5e 53 /*
maenoshin 1:3b12960a1b5e 54 const float L_black = 2.4;//左センサの黒値
maenoshin 1:3b12960a1b5e 55 const float L_white = 1.2;//白値
maenoshin 1:3b12960a1b5e 56 const float R_black = 1.4;//右センサの黒値
maenoshin 1:3b12960a1b5e 57 const float R_white = 0.6;//
maenoshin 1:3b12960a1b5e 58 const float C_black = 2.2(1.5まで);//中心センサの黒値
maenoshin 1:3b12960a1b5e 59 const float C_white = 0.6;//
maenoshin 1:3b12960a1b5e 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
maenoshin 1:3b12960a1b5e 66 float L_target = 1.5;
maenoshin 1:3b12960a1b5e 67 float R_target = 1.5;
maenoshin 1:3b12960a1b5e 68 float C_target = 1.5;
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;//一つ前の状態記号の格納:線を外れた際に引用
maenoshin 1:3b12960a1b5e 184 int mode_change = 0;//modeが変わったら１
komachiangel72 0:5e29061237cd 185
komachiangel72 0:5e29061237cd 186
komachiangel72 0:5e29061237cd 187 enum restart_ignition{//減速後の復帰のための操作に使用する信号
komachiangel72 0:5e29061237cd 188 no=0,
komachiangel72 0:5e29061237cd 189 caution,
komachiangel72 0:5e29061237cd 190 ready,
komachiangel72 0:5e29061237cd 191 yes
komachiangel72 0:5e29061237cd 192 };
komachiangel72 0:5e29061237cd 193
komachiangel72 0:5e29061237cd 194 //int re_ignition;//上restart_ignitionの格納
komachiangel72 0:5e29061237cd 195
komachiangel72 0:5e29061237cd 196 /*
komachiangel72 0:5e29061237cd 197 restart_ignitionについて
komachiangel72 0:5e29061237cd 198
komachiangel72 0:5e29061237cd 199 [re_ignition = no]では、通常の走行状態
komachiangel72 0:5e29061237cd 200 [re_ignition = ready]は急カーブ中の状態
komachiangel72 0:5e29061237cd 201 この状態で中央のセンサーが黒を感知すると[re_ignition = yes]となり、
komachiangel72 0:5e29061237cd 202 一時的にduty比を上げて、走行させる。その後[re_ignition = no]に戻る。
komachiangel72 0:5e29061237cd 203 */
komachiangel72 0:5e29061237cd 204
komachiangel72 0:5e29061237cd 205 //センサーの色判定と走行モード決め
komachiangel72 0:5e29061237cd 206 void Check_reflecter(AnalogIn sensor_R, AnalogIn sensor_L,AnalogIn sensor_C){//リフレクタの黒白判定＆移動方向の決定
komachiangel72 0:5e29061237cd 207 if(mode_debug==1){
komachiangel72 0:5e29061237cd 208 printf("sensor_Right:L432[%.3f]>\n",sensor_R*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 209 printf("sensor_Left:L432[%.3f]>\n",sensor_L*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 210 printf("sensor_Center:L432[%.3f]>\n\n",sensor_C*3.3F);//電圧測定
komachiangel72 0:5e29061237cd 211 }
komachiangel72 0:5e29061237cd 212
komachiangel72 0:5e29061237cd 213 if(sensor_R*3.3F>R_target)
komachiangel72 0:5e29061237cd 214 {
komachiangel72 0:5e29061237cd 215 R_on_off=1;
komachiangel72 0:5e29061237cd 216 }else{
komachiangel72 0:5e29061237cd 217 R_on_off=0;
komachiangel72 0:5e29061237cd 218 }
komachiangel72 0:5e29061237cd 219 if(sensor_L*3.3F>L_target)
komachiangel72 0:5e29061237cd 220 {
komachiangel72 0:5e29061237cd 221 L_on_off=1;
komachiangel72 0:5e29061237cd 222 }else{
komachiangel72 0:5e29061237cd 223 L_on_off=0;
komachiangel72 0:5e29061237cd 224 }
komachiangel72 0:5e29061237cd 225 if(sensor_C*3.3F>C_target)
komachiangel72 0:5e29061237cd 226 {
komachiangel72 0:5e29061237cd 227 C_on_off=1;
komachiangel72 0:5e29061237cd 228 }else{
komachiangel72 0:5e29061237cd 229 C_on_off=0;
komachiangel72 0:5e29061237cd 230 }
komachiangel72 0:5e29061237cd 231
maenoshin 1:3b12960a1b5e 232 if(pre_C_on_off != C_on_off || pre_R_on_off != R_on_off || pre_L_on_off != L_on_off)//modeが変わるか否かの判断
komachiangel72 0:5e29061237cd 233 {
maenoshin 1:3b12960a1b5e 234 mode_change = 1;
komachiangel72 0:5e29061237cd 235 /*if(re_ignition == caution || re_ignition ==ready)
komachiangel72 0:5e29061237cd 236 {
komachiangel72 0:5e29061237cd 237 re_ignition = yes;
komachiangel72 0:5e29061237cd 238 }*/
komachiangel72 0:5e29061237cd 239 if(mode_debug==1)
komachiangel72 0:5e29061237cd 240 {
komachiangel72 0:5e29061237cd 241 printf("Judgement of sensor L[%d] C[%d] R[%d]\n",L_on_off,C_on_off,R_on_off);//[1]なら黒判定
komachiangel72 0:5e29061237cd 242 }
komachiangel72 0:5e29061237cd 243 //frame_counter = 0;
maenoshin 1:3b12960a1b5e 244 }else{
maenoshin 1:3b12960a1b5e 245 //frame_counter++;
maenoshin 1:3b12960a1b5e 246 mode_change = 0;
maenoshin 1:3b12960a1b5e 247 }
maenoshin 1:3b12960a1b5e 248 if(L_on_off==0 && C_on_off==0 && R_on_off==0 && mode_change ==1)//線の外
maenoshin 1:3b12960a1b5e 249 {
maenoshin 1:3b12960a1b5e 250 if(mode == MEDIUM_R || mode == RIGHT || mode == SHARP_R)
maenoshin 1:3b12960a1b5e 251 {
maenoshin 1:3b12960a1b5e 252 //mode = SHARP_R;
maenoshin 1:3b12960a1b5e 253 //Right_high_rotation(sharp_roll_duty_back,sharp_roll_duty_forward);
maenoshin 1:3b12960a1b5e 254 ////re_ignition = ready;
maenoshin 1:3b12960a1b5e 255 mode = MEDIUM_R;
maenoshin 1:3b12960a1b5e 256 machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
maenoshin 1:3b12960a1b5e 257
maenoshin 1:3b12960a1b5e 258 }else if(mode == MEDIUM_L || mode == LEFT || mode == SHARP_L){
maenoshin 1:3b12960a1b5e 259 //mode = SHARP_L;
maenoshin 1:3b12960a1b5e 260 //Right_high_rotation(sharp_roll_duty_forward,sharp_roll_duty_back);
maenoshin 1:3b12960a1b5e 261 //re_ignition = ready;
maenoshin 1:3b12960a1b5e 262 mode = MEDIUM_L;
maenoshin 1:3b12960a1b5e 263 machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
maenoshin 1:3b12960a1b5e 264 }
maenoshin 1:3b12960a1b5e 265
maenoshin 1:3b12960a1b5e 266 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 267 printf("I'm going out of line! Previous condition is %d \n",mode);
maenoshin 1:3b12960a1b5e 268 }
maenoshin 1:3b12960a1b5e 269
komachiangel72 0:5e29061237cd 270 }
maenoshin 1:3b12960a1b5e 271 else if(L_on_off==1 && C_on_off==0 && R_on_off==0 && mode_change ==1)//左折
maenoshin 1:3b12960a1b5e 272 {
maenoshin 1:3b12960a1b5e 273 mode = MEDIUM_L;
maenoshin 1:3b12960a1b5e 274 machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
maenoshin 1:3b12960a1b5e 275 //re_ignition = caution;
maenoshin 1:3b12960a1b5e 276 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 277 printf("M_LEFT\n");
maenoshin 1:3b12960a1b5e 278 }
maenoshin 1:3b12960a1b5e 279 }
maenoshin 1:3b12960a1b5e 280 else if(L_on_off==0 && C_on_off==0 && R_on_off==1 && mode_change ==1)//右折
maenoshin 1:3b12960a1b5e 281 {
maenoshin 1:3b12960a1b5e 282 mode = MEDIUM_R;
maenoshin 1:3b12960a1b5e 283 machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
maenoshin 1:3b12960a1b5e 284 //re_ignition = caution;
maenoshin 1:3b12960a1b5e 285 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 286 printf("M_RIGHT\n");
maenoshin 1:3b12960a1b5e 287 }
maenoshin 1:3b12960a1b5e 288 }
maenoshin 1:3b12960a1b5e 289 else if(L_on_off==1 && C_on_off==0 && R_on_off==1 && mode_change ==1)//黒白黒、エラー、back
maenoshin 1:3b12960a1b5e 290 {
maenoshin 1:3b12960a1b5e 291 //mode = REVERSE;
maenoshin 1:3b12960a1b5e 292 //re_ignition = no;
maenoshin 1:3b12960a1b5e 293 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 294 printf("REVERSE\n");
maenoshin 1:3b12960a1b5e 295 }
maenoshin 1:3b12960a1b5e 296 }
maenoshin 1:3b12960a1b5e 297 else if(L_on_off==0 && C_on_off==1 && R_on_off==0 && mode_change ==1)//前進
maenoshin 1:3b12960a1b5e 298 {
maenoshin 1:3b12960a1b5e 299 //re_ignition = no;
maenoshin 1:3b12960a1b5e 300 machine_Forward(R_duty_basic,L_duty_basic);
maenoshin 1:3b12960a1b5e 301 mode = STRAIGHT;
maenoshin 1:3b12960a1b5e 302 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 303 printf("STRAIGHT\n");
maenoshin 1:3b12960a1b5e 304 }
maenoshin 1:3b12960a1b5e 305 }
maenoshin 1:3b12960a1b5e 306 else if(L_on_off==1 && C_on_off==1 && R_on_off==0 && mode_change ==1)//左折
maenoshin 1:3b12960a1b5e 307 {
maenoshin 1:3b12960a1b5e 308 //re_ignition =no;
maenoshin 1:3b12960a1b5e 309 machine_Forward(roll_duty_large,roll_duty_small);
maenoshin 1:3b12960a1b5e 310 mode = LEFT;
maenoshin 1:3b12960a1b5e 311 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 312 printf("LEFT\n");
maenoshin 1:3b12960a1b5e 313 }
maenoshin 1:3b12960a1b5e 314 }
maenoshin 1:3b12960a1b5e 315 else if(L_on_off==0 && C_on_off==1 && R_on_off==1 && mode_change ==1)//右折
maenoshin 1:3b12960a1b5e 316 {
maenoshin 1:3b12960a1b5e 317 //re_ignition = no;
maenoshin 1:3b12960a1b5e 318 machine_Forward(roll_duty_small,roll_duty_large);
maenoshin 1:3b12960a1b5e 319 mode = RIGHT;
maenoshin 1:3b12960a1b5e 320 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 321 printf("RIGHT\n");
maenoshin 1:3b12960a1b5e 322 }
maenoshin 1:3b12960a1b5e 323 }
maenoshin 1:3b12960a1b5e 324 else if(L_on_off==1 && C_on_off==1 && R_on_off==1 && mode_change ==1)//all black（空中or十字部)よって直進
maenoshin 1:3b12960a1b5e 325 {
maenoshin 1:3b12960a1b5e 326 //re_ignition = no;
maenoshin 1:3b12960a1b5e 327 machine_Forward(R_duty_basic,L_duty_basic);
maenoshin 1:3b12960a1b5e 328 mode = STRAIGHT;
maenoshin 1:3b12960a1b5e 329 if(mode_debug==1){
maenoshin 1:3b12960a1b5e 330 printf("STRAIGHT\n");
maenoshin 1:3b12960a1b5e 331 }
maenoshin 1:3b12960a1b5e 332 }
maenoshin 1:3b12960a1b5e 333
maenoshin 1:3b12960a1b5e 334
komachiangel72 0:5e29061237cd 335 }
komachiangel72 0:5e29061237cd 336
komachiangel72 0:5e29061237cd 337
komachiangel72 0:5e29061237cd 338
komachiangel72 0:5e29061237cd 339 //距離計算関数
komachiangel72 0:5e29061237cd 340 void distance_calculate (void)
komachiangel72 0:5e29061237cd 341 {
komachiangel72 0:5e29061237cd 342 distance_per_frame = (R_rot+L_rot)/2.0f*182.12f;//右の回転数と左の回転数の平均に、一周の距離をかける
komachiangel72 0:5e29061237cd 343 total_distance += distance_per_frame;
komachiangel72 0:5e29061237cd 344 printf("Total Distance [%f] Distance per Frame [%f]\n",total_distance,distance_per_frame);
komachiangel72 0:5e29061237cd 345 printf("Left and right rotation speed per frame R[%f] L[%f]\n",R_rot,L_rot);
komachiangel72 0:5e29061237cd 346
komachiangel72 0:5e29061237cd 347
komachiangel72 0:5e29061237cd 348 }
komachiangel72 0:5e29061237cd 349
komachiangel72 0:5e29061237cd 350 //LED点灯プログラム
komachiangel72 0:5e29061237cd 351 void led_lightning(float total_distance)
komachiangel72 0:5e29061237cd 352 {
komachiangel72 0:5e29061237cd 353 int period_100mm =0;
komachiangel72 0:5e29061237cd 354 for (int i =0;i<500;i++)
komachiangel72 0:5e29061237cd 355 {
komachiangel72 0:5e29061237cd 356 if( total_distance >= (float)i*200 && total_distance < ((float)i+1)*200)
komachiangel72 0:5e29061237cd 357 {
komachiangel72 0:5e29061237cd 358 period_100mm = i;
komachiangel72 0:5e29061237cd 359 break;
komachiangel72 0:5e29061237cd 360 }
komachiangel72 0:5e29061237cd 361 }
komachiangel72 0:5e29061237cd 362 if(period_100mm%2 == 1)
komachiangel72 0:5e29061237cd 363 {
komachiangel72 0:5e29061237cd 364 myled = 1;
komachiangel72 0:5e29061237cd 365 printf("LED IS [ON]\n");
komachiangel72 0:5e29061237cd 366
komachiangel72 0:5e29061237cd 367 }else
komachiangel72 0:5e29061237cd 368 {
komachiangel72 0:5e29061237cd 369 myled = 0;
komachiangel72 0:5e29061237cd 370 printf("LED IS [OFF]\n\n\n");
komachiangel72 0:5e29061237cd 371 }
komachiangel72 0:5e29061237cd 372 }
komachiangel72 0:5e29061237cd 373
komachiangel72 0:5e29061237cd 374
komachiangel72 0:5e29061237cd 375 void memorize_privious_value(void){
komachiangel72 0:5e29061237cd 376 pre_mode = mode;
komachiangel72 0:5e29061237cd 377
komachiangel72 0:5e29061237cd 378 pre_C_on_off = C_on_off;
komachiangel72 0:5e29061237cd 379 pre_R_on_off = R_on_off;
komachiangel72 0:5e29061237cd 380 pre_L_on_off = L_on_off;
komachiangel72 0:5e29061237cd 381
komachiangel72 0:5e29061237cd 382 pre_counter_R = counter_R;
komachiangel72 0:5e29061237cd 383 pre_counter_L = counter_L;
komachiangel72 0:5e29061237cd 384 pre_R_rot=R_rot;
komachiangel72 0:5e29061237cd 385 pre_R_rot=R_rot;
komachiangel72 0:5e29061237cd 386 }
komachiangel72 0:5e29061237cd 387
komachiangel72 0:5e29061237cd 388 int main() {
komachiangel72 0:5e29061237cd 389 AIN1.period(0.001);
komachiangel72 0:5e29061237cd 390 AIN2.period(0.001);
komachiangel72 0:5e29061237cd 391 BIN1.period(0.001);
komachiangel72 0:5e29061237cd 392 BIN2.period(0.001);
komachiangel72 0:5e29061237cd 393
komachiangel72 0:5e29061237cd 394 power_on_led=1;
komachiangel72 0:5e29061237cd 395 machine_Stop();
komachiangel72 0:5e29061237cd 396 mode = STRAIGHT;
komachiangel72 0:5e29061237cd 397 //re_ignition=no;
komachiangel72 0:5e29061237cd 398
komachiangel72 0:5e29061237cd 399 while(1) {
komachiangel72 0:5e29061237cd 400 // wait(delta);
komachiangel72 0:5e29061237cd 401 Check_reflecter(sensor_R,sensor_L,sensor_C);
komachiangel72 0:5e29061237cd 402
komachiangel72 0:5e29061237cd 403 distance_calculate();
komachiangel72 0:5e29061237cd 404
komachiangel72 0:5e29061237cd 405 memorize_privious_value();
komachiangel72 0:5e29061237cd 406 led_lightning(total_distance);
komachiangel72 0:5e29061237cd 407 }
komachiangel72 0:5e29061237cd 408 }