本番(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?
User | Revision | Line number | New 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 センサ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 センサ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 | } |