masamitsu shibata / Mbed 2 deprecated traser5

a

Dependencies:   MotorDriver_SU mbed

line5.cpp@0:b9f6821eb6b5, 2014-11-17 (annotated)

Committer:
12f2025e
Date:
Mon Nov 17 07:02:19 2014 +0000
Revision:
0:b9f6821eb6b5
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12f2025e 0:b9f6821eb6b5 1 #include "mbed.h"
12f2025e 0:b9f6821eb6b5 2 #include "MotorDriver_SU.h"
12f2025e 0:b9f6821eb6b5 3
12f2025e 0:b9f6821eb6b5 4 DigitalIn sw(P0_1, PullUp);
12f2025e 0:b9f6821eb6b5 5 DigitalIn rightline(P0_12);
12f2025e 0:b9f6821eb6b5 6 DigitalIn leftline(P0_14);
12f2025e 0:b9f6821eb6b5 7 DigitalIn rightlift(P0_22);
12f2025e 0:b9f6821eb6b5 8 DigitalIn leftlift(P0_23);
12f2025e 0:b9f6821eb6b5 9 DigitalOut led1(P1_20); //left
12f2025e 0:b9f6821eb6b5 10 DigitalOut led2(P1_27);
12f2025e 0:b9f6821eb6b5 11 DigitalOut led3(P1_21);
12f2025e 0:b9f6821eb6b5 12 DigitalOut led4(P1_26); //right
12f2025e 0:b9f6821eb6b5 13
12f2025e 0:b9f6821eb6b5 14 MotorDriver_SU motor(MOTOR_DOUBLE);
12f2025e 0:b9f6821eb6b5 15 MotorDriver_SU lift(MOTOR_SINGLE);
12f2025e 0:b9f6821eb6b5 16
12f2025e 0:b9f6821eb6b5 17 int a = 0;
12f2025e 0:b9f6821eb6b5 18
12f2025e 0:b9f6821eb6b5 19 int main(){
12f2025e 0:b9f6821eb6b5 20 while(1){
12f2025e 0:b9f6821eb6b5 21 while(sw == 0){
12f2025e 0:b9f6821eb6b5 22 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 23 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 24 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 25 }
12f2025e 0:b9f6821eb6b5 26 while(sw == 1){
12f2025e 0:b9f6821eb6b5 27 if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 1)){ //WWBW
12f2025e 0:b9f6821eb6b5 28 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 29 motor.Drive(1, CW, 2200);
12f2025e 0:b9f6821eb6b5 30 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 31 led1 = 0;
12f2025e 0:b9f6821eb6b5 32 led2 = 0;
12f2025e 0:b9f6821eb6b5 33 led3 = 1;
12f2025e 0:b9f6821eb6b5 34 led4 = 0;
12f2025e 0:b9f6821eb6b5 35 }
12f2025e 0:b9f6821eb6b5 36
12f2025e 0:b9f6821eb6b5 37 else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 0)){ //BBBB
12f2025e 0:b9f6821eb6b5 38 if(a == 0){
12f2025e 0:b9f6821eb6b5 39 led1 = 1;
12f2025e 0:b9f6821eb6b5 40 led2 = 1;
12f2025e 0:b9f6821eb6b5 41 led3 = 1;
12f2025e 0:b9f6821eb6b5 42 led4 = 1;
12f2025e 0:b9f6821eb6b5 43 motor.Drive(0, CW, 2000); //0 = rightmotor, 1 = leftmotor
12f2025e 0:b9f6821eb6b5 44 motor.Drive(1, CW, 2000);
12f2025e 0:b9f6821eb6b5 45 lift.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 46 wait(1.5);
12f2025e 0:b9f6821eb6b5 47 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 48 a = 1;
12f2025e 0:b9f6821eb6b5 49 }
12f2025e 0:b9f6821eb6b5 50 else{
12f2025e 0:b9f6821eb6b5 51 led1 = 1;
12f2025e 0:b9f6821eb6b5 52 led2 = 1;
12f2025e 0:b9f6821eb6b5 53 led3 = 1;
12f2025e 0:b9f6821eb6b5 54 led4 = 1;
12f2025e 0:b9f6821eb6b5 55 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 56 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 57 lift.Drive(0, CW, 4095);
12f2025e 0:b9f6821eb6b5 58 wait(1.5);
12f2025e 0:b9f6821eb6b5 59 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 60 motor.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 61 motor.Drive(1, CCW, 4095);
12f2025e 0:b9f6821eb6b5 62 wait(1);
12f2025e 0:b9f6821eb6b5 63 }
12f2025e 0:b9f6821eb6b5 64 }
12f2025e 0:b9f6821eb6b5 65 else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 0)){ //BBBW
12f2025e 0:b9f6821eb6b5 66 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 67 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 68 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 69 led1 = 1;
12f2025e 0:b9f6821eb6b5 70 led2 = 1;
12f2025e 0:b9f6821eb6b5 71 led3 = 1;
12f2025e 0:b9f6821eb6b5 72 led4 = 0;
12f2025e 0:b9f6821eb6b5 73 }
12f2025e 0:b9f6821eb6b5 74 else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 1)){ //WBBB
12f2025e 0:b9f6821eb6b5 75 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 76 motor.Drive(1, CW, 2200);
12f2025e 0:b9f6821eb6b5 77 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 78 led1 = 0;
12f2025e 0:b9f6821eb6b5 79 led2 = 1;
12f2025e 0:b9f6821eb6b5 80 led3 = 1;
12f2025e 0:b9f6821eb6b5 81 led4 = 1;
12f2025e 0:b9f6821eb6b5 82 }
12f2025e 0:b9f6821eb6b5 83 else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 1)){ //WBBW *
12f2025e 0:b9f6821eb6b5 84 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 85 motor.Drive(1, CW, 1100);
12f2025e 0:b9f6821eb6b5 86 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 87 led1 = 0;
12f2025e 0:b9f6821eb6b5 88 led2 = 1;
12f2025e 0:b9f6821eb6b5 89 led3 = 1;
12f2025e 0:b9f6821eb6b5 90 led4 = 0;
12f2025e 0:b9f6821eb6b5 91 }
12f2025e 0:b9f6821eb6b5 92
12f2025e 0:b9f6821eb6b5 93 else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 0)){ //BWWW *
12f2025e 0:b9f6821eb6b5 94 motor.Drive(0, CW, 1500);
12f2025e 0:b9f6821eb6b5 95 motor.Drive(1, CW, 1000);
12f2025e 0:b9f6821eb6b5 96 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 97 led1 = 1;
12f2025e 0:b9f6821eb6b5 98 led2 = 0;
12f2025e 0:b9f6821eb6b5 99 led3 = 0;
12f2025e 0:b9f6821eb6b5 100 led4 = 0;
12f2025e 0:b9f6821eb6b5 101 }
12f2025e 0:b9f6821eb6b5 102 else if((rightline == 1 && leftline == 1) && (rightlift == 0 && leftlift == 1)){ //WWWB
12f2025e 0:b9f6821eb6b5 103 motor.Drive(0, CW, 500);
12f2025e 0:b9f6821eb6b5 104 motor.Drive(1, CW, 1500);
12f2025e 0:b9f6821eb6b5 105 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 106 led1 = 0;
12f2025e 0:b9f6821eb6b5 107 led2 = 0;
12f2025e 0:b9f6821eb6b5 108 led3 = 0;
12f2025e 0:b9f6821eb6b5 109 led4 = 1;
12f2025e 0:b9f6821eb6b5 110 }
12f2025e 0:b9f6821eb6b5 111 else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 1)){ //WWWW
12f2025e 0:b9f6821eb6b5 112 motor.Drive(0, CCW, 650);
12f2025e 0:b9f6821eb6b5 113 motor.Drive(1, CW, 1500);
12f2025e 0:b9f6821eb6b5 114 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 115 led1 = 0;
12f2025e 0:b9f6821eb6b5 116 led2 = 0;
12f2025e 0:b9f6821eb6b5 117 led3 = 0;
12f2025e 0:b9f6821eb6b5 118 led4 = 0;
12f2025e 0:b9f6821eb6b5 119 }
12f2025e 0:b9f6821eb6b5 120
12f2025e 0:b9f6821eb6b5 121 else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 0)){ //BWBB
12f2025e 0:b9f6821eb6b5 122 if(a == 0){
12f2025e 0:b9f6821eb6b5 123 led1 = 1;
12f2025e 0:b9f6821eb6b5 124 led2 = 0;
12f2025e 0:b9f6821eb6b5 125 led3 = 1;
12f2025e 0:b9f6821eb6b5 126 led4 = 1;
12f2025e 0:b9f6821eb6b5 127 motor.Drive(0, CW, 2000);
12f2025e 0:b9f6821eb6b5 128 motor.Drive(1, CW, 2000);
12f2025e 0:b9f6821eb6b5 129 lift.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 130 wait(1.5);
12f2025e 0:b9f6821eb6b5 131 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 132 a = 1;
12f2025e 0:b9f6821eb6b5 133 }
12f2025e 0:b9f6821eb6b5 134 else{
12f2025e 0:b9f6821eb6b5 135 led1 = 1;
12f2025e 0:b9f6821eb6b5 136 led2 = 0;
12f2025e 0:b9f6821eb6b5 137 led3 = 1;
12f2025e 0:b9f6821eb6b5 138 led4 = 1;
12f2025e 0:b9f6821eb6b5 139 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 140 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 141 lift.Drive(0, CW, 4095);
12f2025e 0:b9f6821eb6b5 142 wait(1.5);
12f2025e 0:b9f6821eb6b5 143 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 144 motor.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 145 motor.Drive(1, CCW, 4095);
12f2025e 0:b9f6821eb6b5 146 wait(1);
12f2025e 0:b9f6821eb6b5 147 }
12f2025e 0:b9f6821eb6b5 148 }
12f2025e 0:b9f6821eb6b5 149 else if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 0)){ //BWBW
12f2025e 0:b9f6821eb6b5 150 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 151 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 152 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 153 led1 = 1;
12f2025e 0:b9f6821eb6b5 154 led2 = 0;
12f2025e 0:b9f6821eb6b5 155 led3 = 1;
12f2025e 0:b9f6821eb6b5 156 led4 = 0;
12f2025e 0:b9f6821eb6b5 157 }
12f2025e 0:b9f6821eb6b5 158 else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 1)){ //WWBB
12f2025e 0:b9f6821eb6b5 159 motor.Drive(0, CW, 1000);
12f2025e 0:b9f6821eb6b5 160 motor.Drive(1, CW, 1500);
12f2025e 0:b9f6821eb6b5 161 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 162 led1 = 0;
12f2025e 0:b9f6821eb6b5 163 led2 = 0;
12f2025e 0:b9f6821eb6b5 164 led3 = 1;
12f2025e 0:b9f6821eb6b5 165 led4 = 1;
12f2025e 0:b9f6821eb6b5 166 }
12f2025e 0:b9f6821eb6b5 167
12f2025e 0:b9f6821eb6b5 168 else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 0)){ //BBWB
12f2025e 0:b9f6821eb6b5 169 if(a == 0){
12f2025e 0:b9f6821eb6b5 170 led1 = 1;
12f2025e 0:b9f6821eb6b5 171 led2 = 1;
12f2025e 0:b9f6821eb6b5 172 led3 = 0;
12f2025e 0:b9f6821eb6b5 173 led4 = 1;
12f2025e 0:b9f6821eb6b5 174 motor.Drive(0, CW, 2000);
12f2025e 0:b9f6821eb6b5 175 motor.Drive(1, CW, 2000);
12f2025e 0:b9f6821eb6b5 176 lift.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 177 wait(1.5);
12f2025e 0:b9f6821eb6b5 178 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 179 a = 1;
12f2025e 0:b9f6821eb6b5 180 }
12f2025e 0:b9f6821eb6b5 181 else{
12f2025e 0:b9f6821eb6b5 182 led1 = 1;
12f2025e 0:b9f6821eb6b5 183 led2 = 1;
12f2025e 0:b9f6821eb6b5 184 led3 = 0;
12f2025e 0:b9f6821eb6b5 185 led4 = 1;
12f2025e 0:b9f6821eb6b5 186 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 187 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 188 lift.Drive(0, CW, 4095);
12f2025e 0:b9f6821eb6b5 189 wait(1.5);
12f2025e 0:b9f6821eb6b5 190 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 191 motor.Drive(0, CCW, 4095);
12f2025e 0:b9f6821eb6b5 192 motor.Drive(1, CCW, 4095);
12f2025e 0:b9f6821eb6b5 193 wait(1);
12f2025e 0:b9f6821eb6b5 194 }
12f2025e 0:b9f6821eb6b5 195 }
12f2025e 0:b9f6821eb6b5 196 else if((rightline == 1 && leftline == 0) && (rightlift == 1 && leftlift == 0)){ //BBWW
12f2025e 0:b9f6821eb6b5 197 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 198 motor.Drive(1, STOP, 0);
12f2025e 0:b9f6821eb6b5 199 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 200 led1 = 1;
12f2025e 0:b9f6821eb6b5 201 led2 = 1;
12f2025e 0:b9f6821eb6b5 202 led3 = 0;
12f2025e 0:b9f6821eb6b5 203 led4 = 0;
12f2025e 0:b9f6821eb6b5 204 }
12f2025e 0:b9f6821eb6b5 205 else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 1)){ //WBWB
12f2025e 0:b9f6821eb6b5 206 motor.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 207 motor.Drive(1, CW, 2200);
12f2025e 0:b9f6821eb6b5 208 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 209 led1 = 0;
12f2025e 0:b9f6821eb6b5 210 led2 = 1;
12f2025e 0:b9f6821eb6b5 211 led3 = 0;
12f2025e 0:b9f6821eb6b5 212 led4 = 1;
12f2025e 0:b9f6821eb6b5 213 }
12f2025e 0:b9f6821eb6b5 214 else{ //WBWW
12f2025e 0:b9f6821eb6b5 215 motor.Drive(0, CW, 2200);
12f2025e 0:b9f6821eb6b5 216 motor.Drive(1, CW, 1000);
12f2025e 0:b9f6821eb6b5 217 lift.Drive(0, STOP, 0);
12f2025e 0:b9f6821eb6b5 218 led1 = 0;
12f2025e 0:b9f6821eb6b5 219 led2 = 1;
12f2025e 0:b9f6821eb6b5 220 led3 = 0;
12f2025e 0:b9f6821eb6b5 221 led4 = 0;
12f2025e 0:b9f6821eb6b5 222 }
12f2025e 0:b9f6821eb6b5 223 }
12f2025e 0:b9f6821eb6b5 224 }
12f2025e 0:b9f6821eb6b5 225 }