last
Dependencies: MotorDriver_SU mbed
Fork of finalcenter by
center.cpp@1:0cd50a576350, 2014-11-23 (annotated)
- Committer:
- 12f2039e
- Date:
- Sun Nov 23 09:08:39 2014 +0000
- Revision:
- 1:0cd50a576350
- Parent:
- 0:cc0c03ee56b3
last
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12F2041G | 0:cc0c03ee56b3 | 1 | #include "mbed.h" |
12F2041G | 0:cc0c03ee56b3 | 2 | #include "MotorDriver_SU.h" |
12F2041G | 0:cc0c03ee56b3 | 3 | |
12F2041G | 0:cc0c03ee56b3 | 4 | DigitalIn sw(P0_1, PullUp); |
12F2041G | 0:cc0c03ee56b3 | 5 | // trace L to R |
12F2041G | 0:cc0c03ee56b3 | 6 | DigitalIn sensor1(P0_16); |
12F2041G | 0:cc0c03ee56b3 | 7 | DigitalIn sensor2(P0_13); |
12F2041G | 0:cc0c03ee56b3 | 8 | DigitalIn sensor3(P0_11); |
12F2041G | 0:cc0c03ee56b3 | 9 | // lift L to R |
12F2041G | 0:cc0c03ee56b3 | 10 | DigitalIn sensor4(P0_21); |
12F2041G | 0:cc0c03ee56b3 | 11 | DigitalIn sensor5(P0_22); |
12F2041G | 0:cc0c03ee56b3 | 12 | |
12F2041G | 0:cc0c03ee56b3 | 13 | DigitalOut led(P1_28); |
12F2041G | 0:cc0c03ee56b3 | 14 | |
12F2041G | 0:cc0c03ee56b3 | 15 | MotorDriver_SU motor(MOTOR_DOUBLE); |
12F2041G | 0:cc0c03ee56b3 | 16 | MotorDriver_SU lift(MOTOR_SINGLE); |
12F2041G | 0:cc0c03ee56b3 | 17 | |
12F2041G | 0:cc0c03ee56b3 | 18 | int main(){ |
12F2041G | 0:cc0c03ee56b3 | 19 | while(1){ |
12F2041G | 0:cc0c03ee56b3 | 20 | |
12F2041G | 0:cc0c03ee56b3 | 21 | //int height = 0;//lift position |
12F2041G | 0:cc0c03ee56b3 | 22 | int check1 = 0;//convert count |
12F2041G | 0:cc0c03ee56b3 | 23 | int check2 = 0; |
12F2041G | 0:cc0c03ee56b3 | 24 | int count = 0;//count liftsensor B&B |
12F2041G | 0:cc0c03ee56b3 | 25 | while(sw == 0){ |
12F2041G | 0:cc0c03ee56b3 | 26 | motor.Drive(0, STOP, 0); //R |
12F2041G | 0:cc0c03ee56b3 | 27 | motor.Drive(1, STOP, 0);//L |
12F2041G | 0:cc0c03ee56b3 | 28 | lift.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 29 | } |
12F2041G | 0:cc0c03ee56b3 | 30 | ////////////////////////////////////////////////////////// |
12F2041G | 0:cc0c03ee56b3 | 31 | //need to change some numerical value after test |
12F2041G | 0:cc0c03ee56b3 | 32 | ////////////////////////////////////////////////////////// |
12F2041G | 0:cc0c03ee56b3 | 33 | while(sw == 1){ |
12F2041G | 0:cc0c03ee56b3 | 34 | if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ |
12F2041G | 0:cc0c03ee56b3 | 35 | motor.Drive(0, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 36 | motor.Drive(1, CW, 1500); |
12F2041G | 0:cc0c03ee56b3 | 37 | } |
12F2041G | 0:cc0c03ee56b3 | 38 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){ |
12f2039e | 1:0cd50a576350 | 39 | motor.Drive(0, CW, 1700); |
12f2039e | 1:0cd50a576350 | 40 | motor.Drive(1, CW, 1700); |
12F2041G | 0:cc0c03ee56b3 | 41 | lift.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 42 | } |
12F2041G | 0:cc0c03ee56b3 | 43 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){ |
12f2039e | 1:0cd50a576350 | 44 | led = 1;//count kakuninn |
12F2041G | 0:cc0c03ee56b3 | 45 | if( count == 0){ |
12F2041G | 0:cc0c03ee56b3 | 46 | lift.Drive(0, CW, 4095); //lift up |
12F2041G | 0:cc0c03ee56b3 | 47 | motor.Drive(0, STOP, 0); //stop |
12F2041G | 0:cc0c03ee56b3 | 48 | motor.Drive(1, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 49 | wait(1); |
12f2039e | 1:0cd50a576350 | 50 | led = 0;//count kakuninn |
12f2039e | 1:0cd50a576350 | 51 | motor.Drive(0, CW, 2000); //restart |
12f2039e | 1:0cd50a576350 | 52 | motor.Drive(1, CW, 2000); |
12F2041G | 0:cc0c03ee56b3 | 53 | lift.Drive(0, CW, 4095); //lift up more |
12F2041G | 0:cc0c03ee56b3 | 54 | wait(0.7); |
12F2041G | 0:cc0c03ee56b3 | 55 | lift.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 56 | //height = 1;//lift position high |
12F2041G | 0:cc0c03ee56b3 | 57 | count++; |
12F2041G | 0:cc0c03ee56b3 | 58 | } |
12F2041G | 0:cc0c03ee56b3 | 59 | //need to change count[1,2,3...] after test |
12F2041G | 0:cc0c03ee56b3 | 60 | else if(count == 1){ |
12F2041G | 0:cc0c03ee56b3 | 61 | count++; |
12F2041G | 0:cc0c03ee56b3 | 62 | } |
12F2041G | 0:cc0c03ee56b3 | 63 | else if(count == 2){ |
12F2041G | 0:cc0c03ee56b3 | 64 | count++; |
12F2041G | 0:cc0c03ee56b3 | 65 | } |
12F2041G | 0:cc0c03ee56b3 | 66 | else if(count == 3){ |
12F2041G | 0:cc0c03ee56b3 | 67 | motor.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 68 | motor.Drive(1, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 69 | lift.Drive(0, CCW, 2000);//lift down |
12F2041G | 0:cc0c03ee56b3 | 70 | wait(1.3); |
12F2041G | 0:cc0c03ee56b3 | 71 | motor.Drive(0, CCW, 4095);//back for 2 seconds |
12F2041G | 0:cc0c03ee56b3 | 72 | motor.Drive(1, CCW, 4095); |
12F2041G | 0:cc0c03ee56b3 | 73 | lift.Drive(0, STOP, 0); |
12f2039e | 1:0cd50a576350 | 74 | led = 0;//count kakunin |
12F2041G | 0:cc0c03ee56b3 | 75 | wait(2); |
12F2041G | 0:cc0c03ee56b3 | 76 | //height = 0;//lift position low |
12F2041G | 0:cc0c03ee56b3 | 77 | count = 0;//reset |
12F2041G | 0:cc0c03ee56b3 | 78 | } |
12F2041G | 0:cc0c03ee56b3 | 79 | else{ |
12F2041G | 0:cc0c03ee56b3 | 80 | ; |
12F2041G | 0:cc0c03ee56b3 | 81 | } |
12F2041G | 0:cc0c03ee56b3 | 82 | } |
12F2041G | 0:cc0c03ee56b3 | 83 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ |
12F2041G | 0:cc0c03ee56b3 | 84 | motor.Drive(0, CW, 1500); |
12f2039e | 1:0cd50a576350 | 85 | motor.Drive(1, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 86 | } |
12F2041G | 0:cc0c03ee56b3 | 87 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){ |
12F2041G | 0:cc0c03ee56b3 | 88 | motor.Drive(0, CW, 1500); |
12F2041G | 0:cc0c03ee56b3 | 89 | motor.Drive(1, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 90 | } |
12F2041G | 0:cc0c03ee56b3 | 91 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){ |
12F2041G | 0:cc0c03ee56b3 | 92 | motor.Drive(0, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 93 | motor.Drive(1, CW, 1500); |
12F2041G | 0:cc0c03ee56b3 | 94 | } |
12F2041G | 0:cc0c03ee56b3 | 95 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){ |
12f2039e | 1:0cd50a576350 | 96 | motor.Drive(0, CW, 500);//ccw change cw |
12f2039e | 1:0cd50a576350 | 97 | motor.Drive(1, CW, 500);//ccw change cw |
12F2041G | 0:cc0c03ee56b3 | 98 | } |
12F2041G | 0:cc0c03ee56b3 | 99 | /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){ |
12F2041G | 0:cc0c03ee56b3 | 100 | motor.Drive(0, CW, 2048); |
12F2041G | 0:cc0c03ee56b3 | 101 | motor.Drive(1, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 102 | lift.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 103 | }*/ |
12F2041G | 0:cc0c03ee56b3 | 104 | //all body on line's outside |
12F2041G | 0:cc0c03ee56b3 | 105 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){ |
12F2041G | 0:cc0c03ee56b3 | 106 | motor.Drive(0, CCW, 200); |
12F2041G | 0:cc0c03ee56b3 | 107 | motor.Drive(1, CW, 2000); |
12F2041G | 0:cc0c03ee56b3 | 108 | lift.Drive(0, STOP, 0); |
12F2041G | 0:cc0c03ee56b3 | 109 | } |
12F2041G | 0:cc0c03ee56b3 | 110 | //select course condition |
12F2041G | 0:cc0c03ee56b3 | 111 | else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){ |
12F2041G | 0:cc0c03ee56b3 | 112 | if(check1 == 0){ |
12F2041G | 0:cc0c03ee56b3 | 113 | check1 = 1; |
12f2039e | 1:0cd50a576350 | 114 | motor.Drive(0, CW, 1000);//adjust |
12f2039e | 1:0cd50a576350 | 115 | motor.Drive(1, CW, 1000); |
12f2039e | 1:0cd50a576350 | 116 | wait(0.5);//adjust |
12F2041G | 0:cc0c03ee56b3 | 117 | //enter the right courses |
12F2041G | 0:cc0c03ee56b3 | 118 | motor.Drive(0, CW, 500);//change both speed |
12F2041G | 0:cc0c03ee56b3 | 119 | motor.Drive(1, CW, 1500); |
12F2041G | 0:cc0c03ee56b3 | 120 | wait(1); |
12F2041G | 0:cc0c03ee56b3 | 121 | } |
12F2041G | 0:cc0c03ee56b3 | 122 | else{ |
12F2041G | 0:cc0c03ee56b3 | 123 | ;//response only once |
12F2041G | 0:cc0c03ee56b3 | 124 | } |
12F2041G | 0:cc0c03ee56b3 | 125 | } |
12F2041G | 0:cc0c03ee56b3 | 126 | else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){ |
12F2041G | 0:cc0c03ee56b3 | 127 | if(check2 == 0){ |
12F2041G | 0:cc0c03ee56b3 | 128 | check2 = 1; |
12F2041G | 0:cc0c03ee56b3 | 129 | //enter the center course |
12F2041G | 0:cc0c03ee56b3 | 130 | motor.Drive(0, CW, 1500);//change both speed |
12F2041G | 0:cc0c03ee56b3 | 131 | motor.Drive(1, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 132 | wait(1); |
12F2041G | 0:cc0c03ee56b3 | 133 | } |
12f2039e | 1:0cd50a576350 | 134 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) { |
12f2039e | 1:0cd50a576350 | 135 | motor.Drive(0, CW, 500); |
12f2039e | 1:0cd50a576350 | 136 | motor.Drive(1, CW, 500); |
12f2039e | 1:0cd50a576350 | 137 | }//tuika |
12f2039e | 1:0cd50a576350 | 138 | |
12f2039e | 1:0cd50a576350 | 139 | |
12F2041G | 0:cc0c03ee56b3 | 140 | else{ |
12F2041G | 0:cc0c03ee56b3 | 141 | ;//response only once |
12F2041G | 0:cc0c03ee56b3 | 142 | } |
12F2041G | 0:cc0c03ee56b3 | 143 | } |
12f2039e | 1:0cd50a576350 | 144 | |
12F2041G | 0:cc0c03ee56b3 | 145 | else{ |
12F2041G | 0:cc0c03ee56b3 | 146 | motor.Drive(0, CW, 2500);//spin to search correct way |
12F2041G | 0:cc0c03ee56b3 | 147 | motor.Drive(1, CW, 500); |
12F2041G | 0:cc0c03ee56b3 | 148 | } |
12F2041G | 0:cc0c03ee56b3 | 149 | } |
12F2041G | 0:cc0c03ee56b3 | 150 | } |
12F2041G | 0:cc0c03ee56b3 | 151 | } |