1113
Dependencies: MotorDriver_SU mbed
Fork of tracer1106 by
trace.cpp@6:477e7d629ea8, 2014-11-07 (annotated)
- Committer:
- 12f2039e
- Date:
- Fri Nov 07 11:28:31 2014 +0000
- Revision:
- 6:477e7d629ea8
- Child:
- 7:b2adb4c5124b
1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12f2039e | 6:477e7d629ea8 | 1 | #include "mbed.h" |
12f2039e | 6:477e7d629ea8 | 2 | #include "MotorDriver_SU.h" |
12f2039e | 6:477e7d629ea8 | 3 | |
12f2039e | 6:477e7d629ea8 | 4 | DigitalIn sw(P0_1, PullUp); |
12f2039e | 6:477e7d629ea8 | 5 | // trace L to R |
12f2039e | 6:477e7d629ea8 | 6 | DigitalIn sensor1(P0_16); |
12f2039e | 6:477e7d629ea8 | 7 | DigitalIn sensor2(P0_13); |
12f2039e | 6:477e7d629ea8 | 8 | DigitalIn sensor3(P0_11); |
12f2039e | 6:477e7d629ea8 | 9 | // lift L to R |
12f2039e | 6:477e7d629ea8 | 10 | DigitalIn sensor4(P0_21); |
12f2039e | 6:477e7d629ea8 | 11 | DigitalIn sensor5(P0_22); |
12f2039e | 6:477e7d629ea8 | 12 | |
12f2039e | 6:477e7d629ea8 | 13 | DigitalOut led(P1_28); |
12f2039e | 6:477e7d629ea8 | 14 | |
12f2039e | 6:477e7d629ea8 | 15 | MotorDriver_SU motor(MOTOR_DOUBLE); |
12f2039e | 6:477e7d629ea8 | 16 | MotorDriver_SU lift(MOTOR_SINGLE); |
12f2039e | 6:477e7d629ea8 | 17 | |
12f2039e | 6:477e7d629ea8 | 18 | int main(){ |
12f2039e | 6:477e7d629ea8 | 19 | while(1){ |
12f2039e | 6:477e7d629ea8 | 20 | int height = 0;//リフト位置指定 |
12f2039e | 6:477e7d629ea8 | 21 | while(sw == 0){ |
12f2039e | 6:477e7d629ea8 | 22 | motor.Drive(0, STOP, 0); //右タイヤ |
12f2039e | 6:477e7d629ea8 | 23 | motor.Drive(1, STOP, 0);//左タイヤ |
12f2039e | 6:477e7d629ea8 | 24 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 25 | } |
12f2039e | 6:477e7d629ea8 | 26 | while(sw == 1){ |
12f2039e | 6:477e7d629ea8 | 27 | //patern1 |
12f2039e | 6:477e7d629ea8 | 28 | if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ |
12f2039e | 6:477e7d629ea8 | 29 | motor.Drive(0, CW, 0); //右タイヤ |
12f2039e | 6:477e7d629ea8 | 30 | motor.Drive(1, CW, 2000);//左タイヤ |
12f2039e | 6:477e7d629ea8 | 31 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 32 | } |
12f2039e | 6:477e7d629ea8 | 33 | //patern2 |
12f2039e | 6:477e7d629ea8 | 34 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){ |
12f2039e | 6:477e7d629ea8 | 35 | motor.Drive(0, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 36 | motor.Drive(1, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 37 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 38 | } |
12f2039e | 6:477e7d629ea8 | 39 | //patern3 |
12f2039e | 6:477e7d629ea8 | 40 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ |
12f2039e | 6:477e7d629ea8 | 41 | motor.Drive(0, CW, 2000); |
12f2039e | 6:477e7d629ea8 | 42 | motor.Drive(1, CW, 0); |
12f2039e | 6:477e7d629ea8 | 43 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 44 | } |
12f2039e | 6:477e7d629ea8 | 45 | //patern4 |
12f2039e | 6:477e7d629ea8 | 46 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){ |
12f2039e | 6:477e7d629ea8 | 47 | motor.Drive(0, CW, 0); |
12f2039e | 6:477e7d629ea8 | 48 | motor.Drive(1, CW, 1500); |
12f2039e | 6:477e7d629ea8 | 49 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 50 | } |
12f2039e | 6:477e7d629ea8 | 51 | //patern5 |
12f2039e | 6:477e7d629ea8 | 52 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){ |
12f2039e | 6:477e7d629ea8 | 53 | motor.Drive(0, CW, 1500); |
12f2039e | 6:477e7d629ea8 | 54 | motor.Drive(1, CW, 0); |
12f2039e | 6:477e7d629ea8 | 55 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 56 | } |
12f2039e | 6:477e7d629ea8 | 57 | //patern6 |
12f2039e | 6:477e7d629ea8 | 58 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){ |
12f2039e | 6:477e7d629ea8 | 59 | motor.Drive(0, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 60 | motor.Drive(1, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 61 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 62 | } |
12f2039e | 6:477e7d629ea8 | 63 | |
12f2039e | 6:477e7d629ea8 | 64 | //patern7 |
12f2039e | 6:477e7d629ea8 | 65 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){ |
12f2039e | 6:477e7d629ea8 | 66 | motor.Drive(0, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 67 | motor.Drive(1, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 68 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 69 | } |
12f2039e | 6:477e7d629ea8 | 70 | |
12f2039e | 6:477e7d629ea8 | 71 | //patern8 探索回転方向 |
12f2039e | 6:477e7d629ea8 | 72 | //ラインの右側に出た場合 反時計回り |
12f2039e | 6:477e7d629ea8 | 73 | /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){ |
12f2039e | 6:477e7d629ea8 | 74 | motor.Drive(0, CW, 2048); |
12f2039e | 6:477e7d629ea8 | 75 | motor.Drive(1, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 76 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 77 | }*/ |
12f2039e | 6:477e7d629ea8 | 78 | //ラインの左側に出た場合 時計回り |
12f2039e | 6:477e7d629ea8 | 79 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){ |
12f2039e | 6:477e7d629ea8 | 80 | motor.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 81 | motor.Drive(1, CW, 1500); |
12f2039e | 6:477e7d629ea8 | 82 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 83 | } |
12f2039e | 6:477e7d629ea8 | 84 | |
12f2039e | 6:477e7d629ea8 | 85 | //リフト判定 |
12f2039e | 6:477e7d629ea8 | 86 | else if(sensor4 == 0 || sensor5 == 0){ |
12f2039e | 6:477e7d629ea8 | 87 | if( height == 0){ |
12f2039e | 6:477e7d629ea8 | 88 | //ストップ |
12f2039e | 6:477e7d629ea8 | 89 | motor.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 90 | motor.Drive(1, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 91 | |
12f2039e | 6:477e7d629ea8 | 92 | lift.Drive(0, CW, 4000); //リフトアップ |
12f2039e | 6:477e7d629ea8 | 93 | wait(1.0); |
12f2039e | 6:477e7d629ea8 | 94 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 95 | wait(0.2); |
12f2039e | 6:477e7d629ea8 | 96 | //再スタート |
12f2039e | 6:477e7d629ea8 | 97 | motor.Drive(0, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 98 | motor.Drive(1, CW, 1000); |
12f2039e | 6:477e7d629ea8 | 99 | wait(0.2); |
12f2039e | 6:477e7d629ea8 | 100 | height = 1;//lift位置 |
12f2039e | 6:477e7d629ea8 | 101 | } |
12f2039e | 6:477e7d629ea8 | 102 | else if(height == 1){ |
12f2039e | 6:477e7d629ea8 | 103 | motor.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 104 | motor.Drive(1, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 105 | lift.Drive(0, CCW, 2000);//リフトダウン |
12f2039e | 6:477e7d629ea8 | 106 | wait(0.5); |
12f2039e | 6:477e7d629ea8 | 107 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 108 | motor.Drive(0, CCW, 4000); |
12f2039e | 6:477e7d629ea8 | 109 | motor.Drive(1, CCW, 4000); |
12f2039e | 6:477e7d629ea8 | 110 | wait(3); |
12f2039e | 6:477e7d629ea8 | 111 | height = 0; |
12f2039e | 6:477e7d629ea8 | 112 | } |
12f2039e | 6:477e7d629ea8 | 113 | } |
12f2039e | 6:477e7d629ea8 | 114 | |
12f2039e | 6:477e7d629ea8 | 115 | else{ |
12f2039e | 6:477e7d629ea8 | 116 | motor.Drive(0, CW, 500); |
12f2039e | 6:477e7d629ea8 | 117 | motor.Drive(1, CW, 500); |
12f2039e | 6:477e7d629ea8 | 118 | lift.Drive(0, STOP, 0); |
12f2039e | 6:477e7d629ea8 | 119 | } |
12f2039e | 6:477e7d629ea8 | 120 | } |
12f2039e | 6:477e7d629ea8 | 121 | } |
12f2039e | 6:477e7d629ea8 | 122 | } |