圭祐 杉浦 / Mbed 2 deprecated ライントレーサー　termB

本番用

Dependencies:   MotorDriver_SU USBDevice mbed

main.cpp@0:fa4301e041d0, 2014-11-08 (annotated)

Committer:
12f2027a
Date:
Sat Nov 08 10:57:34 2014 +0000
Revision:
0:fa4301e041d0
Child:
1:f5a011cef88b
?????????termB

Who changed what in which revision?

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