圭祐 杉浦 / Mbed 2 deprecated termB-1

本番(11/10)で使用したプログラム

Dependencies:   MotorDriver_SU USBDevice mbed

termB-1.cpp@0:29e77dab275d, 2014-11-10 (annotated)

Committer:
12f2027a
Date:
Mon Nov 10 14:47:08 2014 +0000
Revision:
0:29e77dab275d
termB 1??????11/10???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12f2027a 0:29e77dab275d 1 #include "mbed.h"
12f2027a 0:29e77dab275d 2 #include "MotorDriver_SU.h"
12f2027a 0:29e77dab275d 3
12f2027a 0:29e77dab275d 4
12f2027a 0:29e77dab275d 5 DigitalIn sw(P0_1, PullUp);
12f2027a 0:29e77dab275d 6 DigitalOut led(P1_28);
12f2027a 0:29e77dab275d 7 AnalogIn sw1(P0_22);
12f2027a 0:29e77dab275d 8 AnalogIn sw2(P0_12);
12f2027a 0:29e77dab275d 9 AnalogIn sw3(P0_14);
12f2027a 0:29e77dab275d 10 AnalogIn sw4(P0_11);
12f2027a 0:29e77dab275d 11 AnalogIn sw5(P0_13);
12f2027a 0:29e77dab275d 12
12f2027a 0:29e77dab275d 13 MotorDriver_SU motor(MOTOR_DOUBLE);
12f2027a 0:29e77dab275d 14 MotorDriver_SU lift(MOTOR_SINGLE);
12f2027a 0:29e77dab275d 15
12f2027a 0:29e77dab275d 16
12f2027a 0:29e77dab275d 17 int main( ){
12f2027a 0:29e77dab275d 18 float hoge1;
12f2027a 0:29e77dab275d 19 float hoge2;
12f2027a 0:29e77dab275d 20 float hoge3;
12f2027a 0:29e77dab275d 21 float hoge4;
12f2027a 0:29e77dab275d 22 float hoge5;
12f2027a 0:29e77dab275d 23 int AI = 0;
12f2027a 0:29e77dab275d 24
12f2027a 0:29e77dab275d 25
12f2027a 0:29e77dab275d 26 /********************************/
12f2027a 0:29e77dab275d 27 /********************************/
12f2027a 0:29e77dab275d 28 /********************************/
12f2027a 0:29e77dab275d 29 /*荷物を持ち上げる前のプログラム*/
12f2027a 0:29e77dab275d 30 /********************************/
12f2027a 0:29e77dab275d 31 /********************************/
12f2027a 0:29e77dab275d 32 /********************************/
12f2027a 0:29e77dab275d 33
12f2027a 0:29e77dab275d 34 while(1){
12f2027a 0:29e77dab275d 35
12f2027a 0:29e77dab275d 36 hoge1 = sw1.read();
12f2027a 0:29e77dab275d 37 hoge2 = sw2.read();
12f2027a 0:29e77dab275d 38 hoge3 = sw3.read();
12f2027a 0:29e77dab275d 39 hoge4 = sw4.read();
12f2027a 0:29e77dab275d 40 hoge5 = sw5.read();
12f2027a 0:29e77dab275d 41
12f2027a 0:29e77dab275d 42
12f2027a 0:29e77dab275d 43 /************************************/
12f2027a 0:29e77dab275d 44 /*ラインに沿って走るためのプログラム*/
12f2027a 0:29e77dab275d 45 /************************************/
12f2027a 0:29e77dab275d 46 ///////////まっすぐ走るためのプログラム//////////
12f2027a 0:29e77dab275d 47 /*1.1.1 センサ１が黒*/
12f2027a 0:29e77dab275d 48 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 49 motor.Drive(0, CW, 500);
12f2027a 0:29e77dab275d 50 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 51
12f2027a 0:29e77dab275d 52 }
12f2027a 0:29e77dab275d 53
12f2027a 0:29e77dab275d 54
12f2027a 0:29e77dab275d 55 /*1.1.2 センサ2が黒*/
12f2027a 0:29e77dab275d 56 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 57 motor.Drive(0, CW, 800);
12f2027a 0:29e77dab275d 58 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 59
12f2027a 0:29e77dab275d 60 }
12f2027a 0:29e77dab275d 61
12f2027a 0:29e77dab275d 62
12f2027a 0:29e77dab275d 63 /*1.1.3 センサ3が黒*/
12f2027a 0:29e77dab275d 64 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 65 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 66 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 67
12f2027a 0:29e77dab275d 68 }
12f2027a 0:29e77dab275d 69
12f2027a 0:29e77dab275d 70
12f2027a 0:29e77dab275d 71 /*1.1.4 センサ4が黒*/
12f2027a 0:29e77dab275d 72 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 73 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 74 motor.Drive(1, CW, 800);
12f2027a 0:29e77dab275d 75
12f2027a 0:29e77dab275d 76 }
12f2027a 0:29e77dab275d 77
12f2027a 0:29e77dab275d 78
12f2027a 0:29e77dab275d 79 /*1.1.5 センサ5が黒*/
12f2027a 0:29e77dab275d 80 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 81 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 82 motor.Drive(1, CW, 500);
12f2027a 0:29e77dab275d 83
12f2027a 0:29e77dab275d 84 }
12f2027a 0:29e77dab275d 85
12f2027a 0:29e77dab275d 86 /*1.1.6 センサ2-3が黒*/
12f2027a 0:29e77dab275d 87 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 88 motor.Drive(0, CW, 1500);
12f2027a 0:29e77dab275d 89 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 90
12f2027a 0:29e77dab275d 91 }
12f2027a 0:29e77dab275d 92
12f2027a 0:29e77dab275d 93 /*1.1.7 センサ3-4が黒*/
12f2027a 0:29e77dab275d 94 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 95 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 96 motor.Drive(1, CW, 1500);
12f2027a 0:29e77dab275d 97
12f2027a 0:29e77dab275d 98 }
12f2027a 0:29e77dab275d 99
12f2027a 0:29e77dab275d 100 ///////////大きい右カーブを曲がるためのプログラム//////////
12f2027a 0:29e77dab275d 101 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 102 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
12f2027a 0:29e77dab275d 103 AI = 1;
12f2027a 0:29e77dab275d 104 }
12f2027a 0:29e77dab275d 105
12f2027a 0:29e77dab275d 106 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 107 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 108 AI = 1;
12f2027a 0:29e77dab275d 109 }
12f2027a 0:29e77dab275d 110 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 111 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 112 AI = 1;
12f2027a 0:29e77dab275d 113 }
12f2027a 0:29e77dab275d 114
12f2027a 0:29e77dab275d 115 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 116 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 117 AI = 1;
12f2027a 0:29e77dab275d 118 }
12f2027a 0:29e77dab275d 119
12f2027a 0:29e77dab275d 120 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 121 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 122 AI = 1;
12f2027a 0:29e77dab275d 123 }
12f2027a 0:29e77dab275d 124
12f2027a 0:29e77dab275d 125 /*1.2.5 全部白かつAI = 1*/
12f2027a 0:29e77dab275d 126 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 127 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 128 motor.Drive(1, CCW, 4095);
12f2027a 0:29e77dab275d 129
12f2027a 0:29e77dab275d 130 }
12f2027a 0:29e77dab275d 131
12f2027a 0:29e77dab275d 132 ///////////大きい左カーブを曲がるためのプログラム//////////
12f2027a 0:29e77dab275d 133 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 134 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 135 AI = 2;
12f2027a 0:29e77dab275d 136 }
12f2027a 0:29e77dab275d 137
12f2027a 0:29e77dab275d 138 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 139 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 140 AI = 2;
12f2027a 0:29e77dab275d 141 }
12f2027a 0:29e77dab275d 142
12f2027a 0:29e77dab275d 143 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 144 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
12f2027a 0:29e77dab275d 145 AI = 2;
12f2027a 0:29e77dab275d 146 }
12f2027a 0:29e77dab275d 147
12f2027a 0:29e77dab275d 148 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 149 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 150 AI = 2;
12f2027a 0:29e77dab275d 151 }
12f2027a 0:29e77dab275d 152
12f2027a 0:29e77dab275d 153 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 154 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 155 AI = 2;
12f2027a 0:29e77dab275d 156 }
12f2027a 0:29e77dab275d 157
12f2027a 0:29e77dab275d 158 /*1.3.6 全部白かつAI = 2*/
12f2027a 0:29e77dab275d 159 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 160 motor.Drive(0, CCW, 4095);
12f2027a 0:29e77dab275d 161 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 162
12f2027a 0:29e77dab275d 163 }
12f2027a 0:29e77dab275d 164
12f2027a 0:29e77dab275d 165 /**********************************/
12f2027a 0:29e77dab275d 166 /*停止位置で止まるためのプログラム*/
12f2027a 0:29e77dab275d 167 /**********************************/
12f2027a 0:29e77dab275d 168
12f2027a 0:29e77dab275d 169 /*1.4.1 センサ1-3-5が黒*/
12f2027a 0:29e77dab275d 170 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 171 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 172 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 173 break;
12f2027a 0:29e77dab275d 174
12f2027a 0:29e77dab275d 175 }
12f2027a 0:29e77dab275d 176
12f2027a 0:29e77dab275d 177 /*1.4.2 センサ1-2-5が黒*/
12f2027a 0:29e77dab275d 178 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 179 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 180 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 181 break;
12f2027a 0:29e77dab275d 182
12f2027a 0:29e77dab275d 183 }
12f2027a 0:29e77dab275d 184
12f2027a 0:29e77dab275d 185 /*1.4.3 センサ1-4-5が黒*/
12f2027a 0:29e77dab275d 186 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 187 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 188 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 189 break;
12f2027a 0:29e77dab275d 190 }
12f2027a 0:29e77dab275d 191
12f2027a 0:29e77dab275d 192 /*1.4.4 センサ1-2-3-5が黒*/
12f2027a 0:29e77dab275d 193 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 194 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 195 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 196 break;
12f2027a 0:29e77dab275d 197 }
12f2027a 0:29e77dab275d 198
12f2027a 0:29e77dab275d 199 /*1.4.5 センサ1-3-4-5が黒*/
12f2027a 0:29e77dab275d 200 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 201 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 202 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 203 break;
12f2027a 0:29e77dab275d 204 }
12f2027a 0:29e77dab275d 205 }
12f2027a 0:29e77dab275d 206
12f2027a 0:29e77dab275d 207 ///////////停止//////////
12f2027a 0:29e77dab275d 208 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 209 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 210 wait(0.1);
12f2027a 0:29e77dab275d 211 ///////////リフト上げる//////////
12f2027a 0:29e77dab275d 212 lift.Drive(0, CCW, 4095);
12f2027a 0:29e77dab275d 213 wait(0.8);
12f2027a 0:29e77dab275d 214 ///////////リフト停止//////////
12f2027a 0:29e77dab275d 215 lift.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 216 wait(0.1);
12f2027a 0:29e77dab275d 217 ///////////停止位置を超えるために少し前進//////////
12f2027a 0:29e77dab275d 218 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 219 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 220 wait(1.0);
12f2027a 0:29e77dab275d 221
12f2027a 0:29e77dab275d 222
12f2027a 0:29e77dab275d 223
12f2027a 0:29e77dab275d 224
12f2027a 0:29e77dab275d 225 /********************************/
12f2027a 0:29e77dab275d 226 /********************************/
12f2027a 0:29e77dab275d 227 /********************************/
12f2027a 0:29e77dab275d 228 /*荷物を持ち上げた後のプログラム*/
12f2027a 0:29e77dab275d 229 /********************************/
12f2027a 0:29e77dab275d 230 /********************************/
12f2027a 0:29e77dab275d 231 /********************************/
12f2027a 0:29e77dab275d 232
12f2027a 0:29e77dab275d 233
12f2027a 0:29e77dab275d 234 while(1){
12f2027a 0:29e77dab275d 235
12f2027a 0:29e77dab275d 236 hoge1 = sw1.read();
12f2027a 0:29e77dab275d 237 hoge2 = sw2.read();
12f2027a 0:29e77dab275d 238 hoge3 = sw3.read();
12f2027a 0:29e77dab275d 239 hoge4 = sw4.read();
12f2027a 0:29e77dab275d 240 hoge5 = sw5.read();
12f2027a 0:29e77dab275d 241
12f2027a 0:29e77dab275d 242
12f2027a 0:29e77dab275d 243 /************************************/
12f2027a 0:29e77dab275d 244 /*ラインに沿って走るためのプログラム*/
12f2027a 0:29e77dab275d 245 /************************************/
12f2027a 0:29e77dab275d 246 ///////////まっすぐ走るためのプログラム//////////
12f2027a 0:29e77dab275d 247 /*1.1.1 センサ１が黒*/
12f2027a 0:29e77dab275d 248 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 249 motor.Drive(0, CW, 500);
12f2027a 0:29e77dab275d 250 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 251
12f2027a 0:29e77dab275d 252 }
12f2027a 0:29e77dab275d 253
12f2027a 0:29e77dab275d 254
12f2027a 0:29e77dab275d 255 /*1.1.2 センサ2が黒*/
12f2027a 0:29e77dab275d 256 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 257 motor.Drive(0, CW, 800);
12f2027a 0:29e77dab275d 258 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 259
12f2027a 0:29e77dab275d 260 }
12f2027a 0:29e77dab275d 261
12f2027a 0:29e77dab275d 262
12f2027a 0:29e77dab275d 263 /*1.1.3 センサ3が黒*/
12f2027a 0:29e77dab275d 264 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 265 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 266 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 267
12f2027a 0:29e77dab275d 268 }
12f2027a 0:29e77dab275d 269
12f2027a 0:29e77dab275d 270
12f2027a 0:29e77dab275d 271 /*1.1.4 センサ4が黒*/
12f2027a 0:29e77dab275d 272 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 273 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 274 motor.Drive(1, CW, 800);
12f2027a 0:29e77dab275d 275
12f2027a 0:29e77dab275d 276 }
12f2027a 0:29e77dab275d 277
12f2027a 0:29e77dab275d 278
12f2027a 0:29e77dab275d 279 /*1.1.5 センサ5が黒*/
12f2027a 0:29e77dab275d 280 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 281 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 282 motor.Drive(1, CW, 500);
12f2027a 0:29e77dab275d 283
12f2027a 0:29e77dab275d 284 }
12f2027a 0:29e77dab275d 285
12f2027a 0:29e77dab275d 286 /*1.1.6 センサ2-3が黒*/
12f2027a 0:29e77dab275d 287 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 288 motor.Drive(0, CW, 1500);
12f2027a 0:29e77dab275d 289 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 290
12f2027a 0:29e77dab275d 291 }
12f2027a 0:29e77dab275d 292
12f2027a 0:29e77dab275d 293 /*1.1.7 センサ3-4が黒*/
12f2027a 0:29e77dab275d 294 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 295 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 296 motor.Drive(1, CW, 1500);
12f2027a 0:29e77dab275d 297
12f2027a 0:29e77dab275d 298 }
12f2027a 0:29e77dab275d 299
12f2027a 0:29e77dab275d 300 ///////////大きい右カーブを曲がるためのプログラム//////////
12f2027a 0:29e77dab275d 301 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 302 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
12f2027a 0:29e77dab275d 303 AI = 1;
12f2027a 0:29e77dab275d 304 }
12f2027a 0:29e77dab275d 305
12f2027a 0:29e77dab275d 306 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 307 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 308 AI = 1;
12f2027a 0:29e77dab275d 309 }
12f2027a 0:29e77dab275d 310 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 311 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 312 AI = 1;
12f2027a 0:29e77dab275d 313 }
12f2027a 0:29e77dab275d 314
12f2027a 0:29e77dab275d 315 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 316 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 317 AI = 1;
12f2027a 0:29e77dab275d 318 }
12f2027a 0:29e77dab275d 319
12f2027a 0:29e77dab275d 320 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 321 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:29e77dab275d 322 AI = 1;
12f2027a 0:29e77dab275d 323 }
12f2027a 0:29e77dab275d 324
12f2027a 0:29e77dab275d 325 /*1.2.5 全部白かつAI = 1*/
12f2027a 0:29e77dab275d 326 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 327 motor.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 328 motor.Drive(1, CCW, 4095);
12f2027a 0:29e77dab275d 329
12f2027a 0:29e77dab275d 330 }
12f2027a 0:29e77dab275d 331
12f2027a 0:29e77dab275d 332 ///////////大きい左カーブを曲がるためのプログラム//////////
12f2027a 0:29e77dab275d 333 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 334 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 335 AI = 2;
12f2027a 0:29e77dab275d 336 }
12f2027a 0:29e77dab275d 337
12f2027a 0:29e77dab275d 338 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 339 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 340 AI = 2;
12f2027a 0:29e77dab275d 341 }
12f2027a 0:29e77dab275d 342
12f2027a 0:29e77dab275d 343 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 344 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
12f2027a 0:29e77dab275d 345 AI = 2;
12f2027a 0:29e77dab275d 346 }
12f2027a 0:29e77dab275d 347
12f2027a 0:29e77dab275d 348 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 349 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 350 AI = 2;
12f2027a 0:29e77dab275d 351 }
12f2027a 0:29e77dab275d 352
12f2027a 0:29e77dab275d 353 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:29e77dab275d 354 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 355 AI = 2;
12f2027a 0:29e77dab275d 356 }
12f2027a 0:29e77dab275d 357
12f2027a 0:29e77dab275d 358 /*1.3.6 全部白かつAI = 2*/
12f2027a 0:29e77dab275d 359 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:29e77dab275d 360 motor.Drive(0, CCW, 4095);
12f2027a 0:29e77dab275d 361 motor.Drive(1, CW, 4095);
12f2027a 0:29e77dab275d 362
12f2027a 0:29e77dab275d 363 }
12f2027a 0:29e77dab275d 364
12f2027a 0:29e77dab275d 365 /**********************************/
12f2027a 0:29e77dab275d 366 /*停止位置で止まるためのプログラム*/
12f2027a 0:29e77dab275d 367 /**********************************/
12f2027a 0:29e77dab275d 368
12f2027a 0:29e77dab275d 369 /*1.4.1 センサ1-3-5が黒*/
12f2027a 0:29e77dab275d 370 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 371 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 372 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 373 break;
12f2027a 0:29e77dab275d 374
12f2027a 0:29e77dab275d 375 }
12f2027a 0:29e77dab275d 376
12f2027a 0:29e77dab275d 377 /*1.4.2 センサ1-2-5が黒*/
12f2027a 0:29e77dab275d 378 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 379 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 380 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 381 break;
12f2027a 0:29e77dab275d 382
12f2027a 0:29e77dab275d 383 }
12f2027a 0:29e77dab275d 384
12f2027a 0:29e77dab275d 385 /*1.4.3 センサ1-4-5が黒*/
12f2027a 0:29e77dab275d 386 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 387 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 388 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 389 break;
12f2027a 0:29e77dab275d 390 }
12f2027a 0:29e77dab275d 391
12f2027a 0:29e77dab275d 392 /*1.4.4 センサ1-2-3-5が黒*/
12f2027a 0:29e77dab275d 393 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 394 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 395 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 396 break;
12f2027a 0:29e77dab275d 397 }
12f2027a 0:29e77dab275d 398
12f2027a 0:29e77dab275d 399 /*1.4.5 センサ1-3-4-5が黒*/
12f2027a 0:29e77dab275d 400 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
12f2027a 0:29e77dab275d 401 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 402 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 403 break;
12f2027a 0:29e77dab275d 404 }
12f2027a 0:29e77dab275d 405 }
12f2027a 0:29e77dab275d 406
12f2027a 0:29e77dab275d 407 ///////////停止//////////
12f2027a 0:29e77dab275d 408 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 409 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 410 wait(0.1);
12f2027a 0:29e77dab275d 411 ///////////リフト下げる//////////
12f2027a 0:29e77dab275d 412 lift.Drive(0, CW, 4095);
12f2027a 0:29e77dab275d 413 wait(1.0);
12f2027a 0:29e77dab275d 414 ///////////リフト停止//////////
12f2027a 0:29e77dab275d 415 lift.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 416 wait(0.1);
12f2027a 0:29e77dab275d 417 ///////////バック//////////
12f2027a 0:29e77dab275d 418 motor.Drive(0, CCW, 4095);
12f2027a 0:29e77dab275d 419 motor.Drive(1, CCW, 4095);
12f2027a 0:29e77dab275d 420 wait(3.0);
12f2027a 0:29e77dab275d 421 ///////////モータ停止//////////
12f2027a 0:29e77dab275d 422 motor.Drive(0, STOP, 0);
12f2027a 0:29e77dab275d 423 motor.Drive(1, STOP, 0);
12f2027a 0:29e77dab275d 424 wait(1.0);
12f2027a 0:29e77dab275d 425
12f2027a 0:29e77dab275d 426
12f2027a 0:29e77dab275d 427
12f2027a 0:29e77dab275d 428 }