SCRIBE / Mbed 2 deprecated SCRIBE_stepper

scribe robot working with stepper motors

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed

stepper.cpp@2:63aef644e6d2, 2016-04-23 (annotated)

Committer:
manz
Date:
Sat Apr 23 02:46:30 2016 +0000
Revision:
2:63aef644e6d2
Parent:
0:83acd45a2405
Child:
4:1da4d4050306
bluetooth working with new app

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manz 0:83acd45a2405 1 #include "mbed.h";
manz 0:83acd45a2405 2
manz 0:83acd45a2405 3 DigitalOut yellow_l(p9);
manz 0:83acd45a2405 4 DigitalOut orange_l(p14);
manz 0:83acd45a2405 5 DigitalOut brown_l(p15);
manz 0:83acd45a2405 6 DigitalOut black_l(p16);
manz 0:83acd45a2405 7
manz 0:83acd45a2405 8 DigitalOut yellow_r(p17);
manz 0:83acd45a2405 9 DigitalOut orange_r(p18);
manz 0:83acd45a2405 10 DigitalOut brown_r(p19);
manz 0:83acd45a2405 11 DigitalOut black_r(p20);
manz 0:83acd45a2405 12
manz 0:83acd45a2405 13 int wait_t = 60;
manz 0:83acd45a2405 14 int case_right = 0;
manz 0:83acd45a2405 15 int case_left = 0;
manz 0:83acd45a2405 16
manz 2:63aef644e6d2 17
manz 0:83acd45a2405 18 //StepperMotorUni motor( p17, p18, p19, p20);
manz 0:83acd45a2405 19 int step_f(){
manz 0:83acd45a2405 20 //step 0101
manz 0:83acd45a2405 21 black_l = 0;
manz 0:83acd45a2405 22 orange_l = 0;
manz 0:83acd45a2405 23 brown_l = 1;
manz 0:83acd45a2405 24 yellow_l = 1;
manz 0:83acd45a2405 25 black_r = 1;
manz 0:83acd45a2405 26 orange_r = 0;
manz 0:83acd45a2405 27 brown_r = 0;
manz 0:83acd45a2405 28 yellow_r = 1;
manz 0:83acd45a2405 29
manz 0:83acd45a2405 30 wait_ms(wait_t);
manz 0:83acd45a2405 31
manz 0:83acd45a2405 32 //step 1100
manz 0:83acd45a2405 33 black_l = 0;
manz 0:83acd45a2405 34 orange_l = 1;
manz 0:83acd45a2405 35 brown_l = 1;
manz 0:83acd45a2405 36 yellow_l = 0;
manz 0:83acd45a2405 37 black_r = 1;
manz 0:83acd45a2405 38 orange_r = 1;
manz 0:83acd45a2405 39 brown_r = 0;
manz 0:83acd45a2405 40 yellow_r = 0;
manz 0:83acd45a2405 41
manz 0:83acd45a2405 42 wait_ms(wait_t);
manz 0:83acd45a2405 43
manz 0:83acd45a2405 44 //step 1010
manz 0:83acd45a2405 45 black_l = 1;
manz 0:83acd45a2405 46 orange_l = 1;
manz 0:83acd45a2405 47 brown_l = 0;
manz 0:83acd45a2405 48 yellow_l = 0;
manz 0:83acd45a2405 49 black_r = 0;
manz 0:83acd45a2405 50 orange_r = 1;
manz 0:83acd45a2405 51 brown_r = 1;
manz 0:83acd45a2405 52 yellow_r = 0;
manz 0:83acd45a2405 53
manz 0:83acd45a2405 54 wait_ms(wait_t);
manz 0:83acd45a2405 55
manz 0:83acd45a2405 56 //step 0011
manz 0:83acd45a2405 57 black_l = 1;
manz 0:83acd45a2405 58 orange_l = 0;
manz 0:83acd45a2405 59 brown_l = 0;
manz 0:83acd45a2405 60 yellow_l = 1;
manz 0:83acd45a2405 61 black_r = 0;
manz 0:83acd45a2405 62 orange_r = 0;
manz 0:83acd45a2405 63 brown_r = 1;
manz 0:83acd45a2405 64 yellow_r = 1;
manz 0:83acd45a2405 65
manz 0:83acd45a2405 66 wait_ms(wait_t);
manz 0:83acd45a2405 67
manz 0:83acd45a2405 68 return 1;
manz 0:83acd45a2405 69 }
manz 0:83acd45a2405 70
manz 0:83acd45a2405 71
manz 0:83acd45a2405 72 int step_b(){
manz 0:83acd45a2405 73 //step 0101
manz 0:83acd45a2405 74 black_r = 0;
manz 0:83acd45a2405 75 orange_r = 0;
manz 0:83acd45a2405 76 brown_r = 1;
manz 0:83acd45a2405 77 yellow_r = 1;
manz 0:83acd45a2405 78 black_l = 1;
manz 0:83acd45a2405 79 orange_l = 0;
manz 0:83acd45a2405 80 brown_l = 0;
manz 0:83acd45a2405 81 yellow_l = 1;
manz 0:83acd45a2405 82
manz 0:83acd45a2405 83 wait_ms(wait_t);
manz 0:83acd45a2405 84
manz 0:83acd45a2405 85 //step 1100
manz 0:83acd45a2405 86 black_r = 0;
manz 0:83acd45a2405 87 orange_r = 1;
manz 0:83acd45a2405 88 brown_r = 1;
manz 0:83acd45a2405 89 yellow_r = 0;
manz 0:83acd45a2405 90 black_l = 1;
manz 0:83acd45a2405 91 orange_l = 1;
manz 0:83acd45a2405 92 brown_l = 0;
manz 0:83acd45a2405 93 yellow_l = 0;
manz 0:83acd45a2405 94
manz 0:83acd45a2405 95 wait_ms(wait_t);
manz 0:83acd45a2405 96
manz 0:83acd45a2405 97 //step 1010
manz 0:83acd45a2405 98 black_r = 1;
manz 0:83acd45a2405 99 orange_r = 1;
manz 0:83acd45a2405 100 brown_r = 0;
manz 0:83acd45a2405 101 yellow_r = 0;
manz 0:83acd45a2405 102 black_l = 0;
manz 0:83acd45a2405 103 orange_l = 1;
manz 0:83acd45a2405 104 brown_l = 1;
manz 0:83acd45a2405 105 yellow_l = 0;
manz 0:83acd45a2405 106
manz 0:83acd45a2405 107 wait_ms(wait_t);
manz 0:83acd45a2405 108
manz 0:83acd45a2405 109 //step 0011
manz 0:83acd45a2405 110 black_r = 1;
manz 0:83acd45a2405 111 orange_r = 0;
manz 0:83acd45a2405 112 brown_r = 0;
manz 0:83acd45a2405 113 yellow_r = 1;
manz 0:83acd45a2405 114 black_l = 0;
manz 0:83acd45a2405 115 orange_l = 0;
manz 0:83acd45a2405 116 brown_l = 1;
manz 0:83acd45a2405 117 yellow_l = 1;
manz 0:83acd45a2405 118
manz 0:83acd45a2405 119 wait_ms(wait_t);
manz 0:83acd45a2405 120
manz 0:83acd45a2405 121 return 1;
manz 0:83acd45a2405 122 }
manz 0:83acd45a2405 123
manz 0:83acd45a2405 124 int step_right(){
manz 0:83acd45a2405 125 switch(case_right){
manz 0:83acd45a2405 126 case 0:
manz 0:83acd45a2405 127 //step 0101
manz 0:83acd45a2405 128 black_r = 0;
manz 0:83acd45a2405 129 orange_r = 0;
manz 0:83acd45a2405 130 brown_r = 1;
manz 0:83acd45a2405 131 yellow_r = 1;
manz 0:83acd45a2405 132 black_l = 0;
manz 0:83acd45a2405 133 orange_l = 0;
manz 0:83acd45a2405 134 brown_l = 1;
manz 0:83acd45a2405 135 yellow_l = 1;
manz 0:83acd45a2405 136
manz 0:83acd45a2405 137 wait_ms(wait_t);
manz 0:83acd45a2405 138 case_right = 1;
manz 0:83acd45a2405 139 break;
manz 0:83acd45a2405 140
manz 0:83acd45a2405 141 case 1:
manz 0:83acd45a2405 142 //step 1100
manz 0:83acd45a2405 143 black_r = 0;
manz 0:83acd45a2405 144 orange_r = 1;
manz 0:83acd45a2405 145 brown_r = 1;
manz 0:83acd45a2405 146 yellow_r = 0;
manz 0:83acd45a2405 147 black_l = 0;
manz 0:83acd45a2405 148 orange_l = 1;
manz 0:83acd45a2405 149 brown_l = 1;
manz 0:83acd45a2405 150 yellow_l = 0;
manz 0:83acd45a2405 151
manz 0:83acd45a2405 152 wait_ms(wait_t);
manz 0:83acd45a2405 153 case_right = 2;
manz 0:83acd45a2405 154 break;
manz 0:83acd45a2405 155
manz 0:83acd45a2405 156
manz 0:83acd45a2405 157 case 2:
manz 0:83acd45a2405 158 //step 1010
manz 0:83acd45a2405 159 black_r = 1;
manz 0:83acd45a2405 160 orange_r = 1;
manz 0:83acd45a2405 161 brown_r = 0;
manz 0:83acd45a2405 162 yellow_r = 0;
manz 0:83acd45a2405 163 black_l = 1;
manz 0:83acd45a2405 164 orange_l = 1;
manz 0:83acd45a2405 165 brown_l = 0;
manz 0:83acd45a2405 166 yellow_l = 0;
manz 0:83acd45a2405 167
manz 0:83acd45a2405 168 wait_ms(wait_t);
manz 0:83acd45a2405 169 case_right = 3;
manz 0:83acd45a2405 170 break;
manz 0:83acd45a2405 171
manz 0:83acd45a2405 172
manz 0:83acd45a2405 173 case 3:
manz 0:83acd45a2405 174 //step 0011
manz 0:83acd45a2405 175 black_r = 1;
manz 0:83acd45a2405 176 orange_r = 0;
manz 0:83acd45a2405 177 brown_r = 0;
manz 0:83acd45a2405 178 yellow_r = 1;
manz 0:83acd45a2405 179 black_l = 1;
manz 0:83acd45a2405 180 orange_l = 0;
manz 0:83acd45a2405 181 brown_l = 0;
manz 0:83acd45a2405 182 yellow_l = 1;
manz 0:83acd45a2405 183
manz 0:83acd45a2405 184 wait_ms(wait_t);
manz 0:83acd45a2405 185 case_right = 0;
manz 0:83acd45a2405 186 break;
manz 0:83acd45a2405 187 }
manz 0:83acd45a2405 188 return 1;
manz 0:83acd45a2405 189 }
manz 0:83acd45a2405 190
manz 0:83acd45a2405 191
manz 0:83acd45a2405 192 int step_left(){
manz 0:83acd45a2405 193 switch(case_left){
manz 0:83acd45a2405 194 case 0:
manz 0:83acd45a2405 195 //step 0011
manz 0:83acd45a2405 196 black_l = 1;
manz 0:83acd45a2405 197 orange_l = 0;
manz 0:83acd45a2405 198 brown_l = 0;
manz 0:83acd45a2405 199 yellow_l = 1;
manz 0:83acd45a2405 200 black_r = 1;
manz 0:83acd45a2405 201 orange_r = 0;
manz 0:83acd45a2405 202 brown_r = 0;
manz 0:83acd45a2405 203 yellow_r = 1;
manz 0:83acd45a2405 204
manz 0:83acd45a2405 205 wait_ms(wait_t);
manz 0:83acd45a2405 206 case_left = 1;
manz 0:83acd45a2405 207 break;
manz 0:83acd45a2405 208
manz 0:83acd45a2405 209 case 1:
manz 0:83acd45a2405 210 black_l = 1;
manz 0:83acd45a2405 211 orange_l = 1;
manz 0:83acd45a2405 212 brown_l = 0;
manz 0:83acd45a2405 213 yellow_l = 0;
manz 0:83acd45a2405 214 black_r = 1;
manz 0:83acd45a2405 215 orange_r = 1;
manz 0:83acd45a2405 216 brown_r = 0;
manz 0:83acd45a2405 217 yellow_r = 0;
manz 0:83acd45a2405 218
manz 0:83acd45a2405 219 wait_ms(wait_t);
manz 0:83acd45a2405 220 case_left = 2;
manz 0:83acd45a2405 221 break;
manz 0:83acd45a2405 222
manz 0:83acd45a2405 223 case 2:
manz 0:83acd45a2405 224 black_l = 0;
manz 0:83acd45a2405 225 orange_l = 1;
manz 0:83acd45a2405 226 brown_l = 1;
manz 0:83acd45a2405 227 yellow_l = 0;
manz 0:83acd45a2405 228 black_r = 0;
manz 0:83acd45a2405 229 orange_r = 1;
manz 0:83acd45a2405 230 brown_r = 1;
manz 0:83acd45a2405 231 yellow_r = 0;
manz 0:83acd45a2405 232
manz 0:83acd45a2405 233 wait_ms(wait_t);
manz 0:83acd45a2405 234 case_left = 3;
manz 0:83acd45a2405 235 break;
manz 0:83acd45a2405 236
manz 0:83acd45a2405 237 case 3:
manz 0:83acd45a2405 238 black_l = 0;
manz 0:83acd45a2405 239 orange_l = 0;
manz 0:83acd45a2405 240 brown_l = 1;
manz 0:83acd45a2405 241 yellow_l = 1;
manz 0:83acd45a2405 242 black_r = 0;
manz 0:83acd45a2405 243 orange_r = 0;
manz 0:83acd45a2405 244 brown_r = 1;
manz 0:83acd45a2405 245 yellow_r = 1;
manz 0:83acd45a2405 246
manz 0:83acd45a2405 247 wait_ms(wait_t);
manz 0:83acd45a2405 248
manz 0:83acd45a2405 249 case_left = 0;
manz 0:83acd45a2405 250 break;
manz 0:83acd45a2405 251 }
manz 0:83acd45a2405 252 return 1;
manz 0:83acd45a2405 253 }