SCRIBE / Mbed 2 deprecated SCRIBE_servo

The servo version of SCRIBE

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed

Fork of SCRIBE_stepper by SCRIBE

stepper.cpp@5:1da4d4050306, 2016-04-25 (annotated)

Committer:
manz
Date:
Mon Apr 25 05:38:48 2016 +0000
Revision:
5:1da4d4050306
Parent:
3:63aef644e6d2 
working with new thread system

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 5:1da4d4050306 14 int case_stepf = 0;
manz 0:83acd45a2405 15 int case_right = 0;
manz 0:83acd45a2405 16 int case_left = 0;
manz 5:1da4d4050306 17 int case_rone = 0;
manz 0:83acd45a2405 18
manz 3:63aef644e6d2 19
manz 0:83acd45a2405 20 //StepperMotorUni motor( p17, p18, p19, p20);
manz 0:83acd45a2405 21 int step_f(){
manz 5:1da4d4050306 22 switch(case_stepf){
manz 5:1da4d4050306 23 case 0:
manz 5:1da4d4050306 24 //step 0101
manz 5:1da4d4050306 25 black_l = 0;
manz 5:1da4d4050306 26 orange_l = 0;
manz 5:1da4d4050306 27 brown_l = 1;
manz 5:1da4d4050306 28 yellow_l = 1;
manz 5:1da4d4050306 29 black_r = 1;
manz 5:1da4d4050306 30 orange_r = 0;
manz 5:1da4d4050306 31 brown_r = 0;
manz 5:1da4d4050306 32 yellow_r = 1;
manz 5:1da4d4050306 33
manz 5:1da4d4050306 34 wait_ms(wait_t);
manz 5:1da4d4050306 35 case_stepf=1;
manz 5:1da4d4050306 36 break;
manz 5:1da4d4050306 37
manz 5:1da4d4050306 38 case 1:
manz 5:1da4d4050306 39 //step 1100
manz 5:1da4d4050306 40 black_l = 0;
manz 5:1da4d4050306 41 orange_l = 1;
manz 5:1da4d4050306 42 brown_l = 1;
manz 5:1da4d4050306 43 yellow_l = 0;
manz 5:1da4d4050306 44 black_r = 1;
manz 5:1da4d4050306 45 orange_r = 1;
manz 5:1da4d4050306 46 brown_r = 0;
manz 5:1da4d4050306 47 yellow_r = 0;
manz 5:1da4d4050306 48
manz 5:1da4d4050306 49 wait_ms(wait_t);
manz 5:1da4d4050306 50 case_stepf=2;
manz 5:1da4d4050306 51 break;
manz 5:1da4d4050306 52
manz 5:1da4d4050306 53 case 2:
manz 5:1da4d4050306 54 //step 1010
manz 5:1da4d4050306 55 black_l = 1;
manz 5:1da4d4050306 56 orange_l = 1;
manz 5:1da4d4050306 57 brown_l = 0;
manz 5:1da4d4050306 58 yellow_l = 0;
manz 5:1da4d4050306 59 black_r = 0;
manz 5:1da4d4050306 60 orange_r = 1;
manz 5:1da4d4050306 61 brown_r = 1;
manz 5:1da4d4050306 62 yellow_r = 0;
manz 5:1da4d4050306 63
manz 5:1da4d4050306 64 wait_ms(wait_t);
manz 5:1da4d4050306 65 case_stepf = 3;
manz 5:1da4d4050306 66 break;
manz 5:1da4d4050306 67
manz 5:1da4d4050306 68 case 3:
manz 5:1da4d4050306 69 //step 0011
manz 5:1da4d4050306 70 black_l = 1;
manz 5:1da4d4050306 71 orange_l = 0;
manz 5:1da4d4050306 72 brown_l = 0;
manz 5:1da4d4050306 73 yellow_l = 1;
manz 5:1da4d4050306 74 black_r = 0;
manz 5:1da4d4050306 75 orange_r = 0;
manz 5:1da4d4050306 76 brown_r = 1;
manz 5:1da4d4050306 77 yellow_r = 1;
manz 5:1da4d4050306 78
manz 5:1da4d4050306 79 wait_ms(wait_t);
manz 5:1da4d4050306 80 case_stepf = 0;
manz 5:1da4d4050306 81 break;
manz 5:1da4d4050306 82 }
manz 0:83acd45a2405 83 return 1;
manz 0:83acd45a2405 84 }
manz 0:83acd45a2405 85
manz 0:83acd45a2405 86
manz 0:83acd45a2405 87 int step_b(){
manz 0:83acd45a2405 88 //step 0101
manz 0:83acd45a2405 89 black_r = 0;
manz 0:83acd45a2405 90 orange_r = 0;
manz 0:83acd45a2405 91 brown_r = 1;
manz 0:83acd45a2405 92 yellow_r = 1;
manz 0:83acd45a2405 93 black_l = 1;
manz 0:83acd45a2405 94 orange_l = 0;
manz 0:83acd45a2405 95 brown_l = 0;
manz 0:83acd45a2405 96 yellow_l = 1;
manz 0:83acd45a2405 97
manz 0:83acd45a2405 98 wait_ms(wait_t);
manz 0:83acd45a2405 99
manz 0:83acd45a2405 100 //step 1100
manz 0:83acd45a2405 101 black_r = 0;
manz 0:83acd45a2405 102 orange_r = 1;
manz 0:83acd45a2405 103 brown_r = 1;
manz 0:83acd45a2405 104 yellow_r = 0;
manz 0:83acd45a2405 105 black_l = 1;
manz 0:83acd45a2405 106 orange_l = 1;
manz 0:83acd45a2405 107 brown_l = 0;
manz 0:83acd45a2405 108 yellow_l = 0;
manz 0:83acd45a2405 109
manz 0:83acd45a2405 110 wait_ms(wait_t);
manz 0:83acd45a2405 111
manz 0:83acd45a2405 112 //step 1010
manz 0:83acd45a2405 113 black_r = 1;
manz 0:83acd45a2405 114 orange_r = 1;
manz 0:83acd45a2405 115 brown_r = 0;
manz 0:83acd45a2405 116 yellow_r = 0;
manz 0:83acd45a2405 117 black_l = 0;
manz 0:83acd45a2405 118 orange_l = 1;
manz 0:83acd45a2405 119 brown_l = 1;
manz 0:83acd45a2405 120 yellow_l = 0;
manz 0:83acd45a2405 121
manz 0:83acd45a2405 122 wait_ms(wait_t);
manz 0:83acd45a2405 123
manz 0:83acd45a2405 124 //step 0011
manz 0:83acd45a2405 125 black_r = 1;
manz 0:83acd45a2405 126 orange_r = 0;
manz 0:83acd45a2405 127 brown_r = 0;
manz 0:83acd45a2405 128 yellow_r = 1;
manz 0:83acd45a2405 129 black_l = 0;
manz 0:83acd45a2405 130 orange_l = 0;
manz 0:83acd45a2405 131 brown_l = 1;
manz 0:83acd45a2405 132 yellow_l = 1;
manz 0:83acd45a2405 133
manz 0:83acd45a2405 134 wait_ms(wait_t);
manz 0:83acd45a2405 135
manz 0:83acd45a2405 136 return 1;
manz 0:83acd45a2405 137 }
manz 0:83acd45a2405 138
manz 0:83acd45a2405 139 int step_right(){
manz 0:83acd45a2405 140 switch(case_right){
manz 0:83acd45a2405 141 case 0:
manz 0:83acd45a2405 142 //step 0101
manz 0:83acd45a2405 143 black_r = 0;
manz 0:83acd45a2405 144 orange_r = 0;
manz 0:83acd45a2405 145 brown_r = 1;
manz 0:83acd45a2405 146 yellow_r = 1;
manz 0:83acd45a2405 147 black_l = 0;
manz 0:83acd45a2405 148 orange_l = 0;
manz 0:83acd45a2405 149 brown_l = 1;
manz 0:83acd45a2405 150 yellow_l = 1;
manz 0:83acd45a2405 151
manz 0:83acd45a2405 152 wait_ms(wait_t);
manz 0:83acd45a2405 153 case_right = 1;
manz 0:83acd45a2405 154 break;
manz 0:83acd45a2405 155
manz 0:83acd45a2405 156 case 1:
manz 0:83acd45a2405 157 //step 1100
manz 0:83acd45a2405 158 black_r = 0;
manz 0:83acd45a2405 159 orange_r = 1;
manz 0:83acd45a2405 160 brown_r = 1;
manz 0:83acd45a2405 161 yellow_r = 0;
manz 0:83acd45a2405 162 black_l = 0;
manz 0:83acd45a2405 163 orange_l = 1;
manz 0:83acd45a2405 164 brown_l = 1;
manz 0:83acd45a2405 165 yellow_l = 0;
manz 0:83acd45a2405 166
manz 0:83acd45a2405 167 wait_ms(wait_t);
manz 0:83acd45a2405 168 case_right = 2;
manz 0:83acd45a2405 169 break;
manz 0:83acd45a2405 170
manz 0:83acd45a2405 171
manz 0:83acd45a2405 172 case 2:
manz 0:83acd45a2405 173 //step 1010
manz 0:83acd45a2405 174 black_r = 1;
manz 0:83acd45a2405 175 orange_r = 1;
manz 0:83acd45a2405 176 brown_r = 0;
manz 0:83acd45a2405 177 yellow_r = 0;
manz 0:83acd45a2405 178 black_l = 1;
manz 0:83acd45a2405 179 orange_l = 1;
manz 0:83acd45a2405 180 brown_l = 0;
manz 0:83acd45a2405 181 yellow_l = 0;
manz 0:83acd45a2405 182
manz 0:83acd45a2405 183 wait_ms(wait_t);
manz 0:83acd45a2405 184 case_right = 3;
manz 0:83acd45a2405 185 break;
manz 0:83acd45a2405 186
manz 0:83acd45a2405 187
manz 0:83acd45a2405 188 case 3:
manz 0:83acd45a2405 189 //step 0011
manz 0:83acd45a2405 190 black_r = 1;
manz 0:83acd45a2405 191 orange_r = 0;
manz 0:83acd45a2405 192 brown_r = 0;
manz 0:83acd45a2405 193 yellow_r = 1;
manz 0:83acd45a2405 194 black_l = 1;
manz 0:83acd45a2405 195 orange_l = 0;
manz 0:83acd45a2405 196 brown_l = 0;
manz 0:83acd45a2405 197 yellow_l = 1;
manz 0:83acd45a2405 198
manz 0:83acd45a2405 199 wait_ms(wait_t);
manz 0:83acd45a2405 200 case_right = 0;
manz 0:83acd45a2405 201 break;
manz 0:83acd45a2405 202 }
manz 0:83acd45a2405 203 return 1;
manz 0:83acd45a2405 204 }
manz 0:83acd45a2405 205
manz 0:83acd45a2405 206
manz 0:83acd45a2405 207 int step_left(){
manz 0:83acd45a2405 208 switch(case_left){
manz 0:83acd45a2405 209 case 0:
manz 0:83acd45a2405 210 //step 0011
manz 0:83acd45a2405 211 black_l = 1;
manz 0:83acd45a2405 212 orange_l = 0;
manz 0:83acd45a2405 213 brown_l = 0;
manz 0:83acd45a2405 214 yellow_l = 1;
manz 0:83acd45a2405 215 black_r = 1;
manz 0:83acd45a2405 216 orange_r = 0;
manz 0:83acd45a2405 217 brown_r = 0;
manz 0:83acd45a2405 218 yellow_r = 1;
manz 0:83acd45a2405 219
manz 0:83acd45a2405 220 wait_ms(wait_t);
manz 0:83acd45a2405 221 case_left = 1;
manz 0:83acd45a2405 222 break;
manz 0:83acd45a2405 223
manz 0:83acd45a2405 224 case 1:
manz 0:83acd45a2405 225 black_l = 1;
manz 0:83acd45a2405 226 orange_l = 1;
manz 0:83acd45a2405 227 brown_l = 0;
manz 0:83acd45a2405 228 yellow_l = 0;
manz 0:83acd45a2405 229 black_r = 1;
manz 0:83acd45a2405 230 orange_r = 1;
manz 0:83acd45a2405 231 brown_r = 0;
manz 0:83acd45a2405 232 yellow_r = 0;
manz 0:83acd45a2405 233
manz 0:83acd45a2405 234 wait_ms(wait_t);
manz 0:83acd45a2405 235 case_left = 2;
manz 0:83acd45a2405 236 break;
manz 0:83acd45a2405 237
manz 0:83acd45a2405 238 case 2:
manz 0:83acd45a2405 239 black_l = 0;
manz 0:83acd45a2405 240 orange_l = 1;
manz 0:83acd45a2405 241 brown_l = 1;
manz 0:83acd45a2405 242 yellow_l = 0;
manz 0:83acd45a2405 243 black_r = 0;
manz 0:83acd45a2405 244 orange_r = 1;
manz 0:83acd45a2405 245 brown_r = 1;
manz 0:83acd45a2405 246 yellow_r = 0;
manz 0:83acd45a2405 247
manz 0:83acd45a2405 248 wait_ms(wait_t);
manz 0:83acd45a2405 249 case_left = 3;
manz 0:83acd45a2405 250 break;
manz 0:83acd45a2405 251
manz 0:83acd45a2405 252 case 3:
manz 0:83acd45a2405 253 black_l = 0;
manz 0:83acd45a2405 254 orange_l = 0;
manz 0:83acd45a2405 255 brown_l = 1;
manz 0:83acd45a2405 256 yellow_l = 1;
manz 0:83acd45a2405 257 black_r = 0;
manz 0:83acd45a2405 258 orange_r = 0;
manz 0:83acd45a2405 259 brown_r = 1;
manz 0:83acd45a2405 260 yellow_r = 1;
manz 0:83acd45a2405 261
manz 0:83acd45a2405 262 wait_ms(wait_t);
manz 0:83acd45a2405 263
manz 0:83acd45a2405 264 case_left = 0;
manz 0:83acd45a2405 265 break;
manz 0:83acd45a2405 266 }
manz 0:83acd45a2405 267 return 1;
manz 0:83acd45a2405 268 }
manz 5:1da4d4050306 269
manz 5:1da4d4050306 270 int step_rone(){
manz 5:1da4d4050306 271 switch(case_rone){
manz 5:1da4d4050306 272 case 0:
manz 5:1da4d4050306 273 //step 0101
manz 5:1da4d4050306 274 black_r = 0;
manz 5:1da4d4050306 275 orange_r = 0;
manz 5:1da4d4050306 276 brown_r = 1;
manz 5:1da4d4050306 277 yellow_r = 1;
manz 5:1da4d4050306 278
manz 5:1da4d4050306 279 wait_ms(wait_t);
manz 5:1da4d4050306 280 case_right = 1;
manz 5:1da4d4050306 281 break;
manz 5:1da4d4050306 282
manz 5:1da4d4050306 283 case 1:
manz 5:1da4d4050306 284 //step 1100
manz 5:1da4d4050306 285 black_r = 0;
manz 5:1da4d4050306 286 orange_r = 1;
manz 5:1da4d4050306 287 brown_r = 1;
manz 5:1da4d4050306 288 yellow_r = 0;
manz 5:1da4d4050306 289
manz 5:1da4d4050306 290 wait_ms(wait_t);
manz 5:1da4d4050306 291 case_right = 2;
manz 5:1da4d4050306 292 break;
manz 5:1da4d4050306 293
manz 5:1da4d4050306 294
manz 5:1da4d4050306 295 case 2:
manz 5:1da4d4050306 296 //step 1010
manz 5:1da4d4050306 297 black_r = 1;
manz 5:1da4d4050306 298 orange_r = 1;
manz 5:1da4d4050306 299 brown_r = 0;
manz 5:1da4d4050306 300 yellow_r = 0;
manz 5:1da4d4050306 301
manz 5:1da4d4050306 302 wait_ms(wait_t);
manz 5:1da4d4050306 303 case_right = 3;
manz 5:1da4d4050306 304 break;
manz 5:1da4d4050306 305
manz 5:1da4d4050306 306
manz 5:1da4d4050306 307 case 3:
manz 5:1da4d4050306 308 //step 0011
manz 5:1da4d4050306 309 black_r = 1;
manz 5:1da4d4050306 310 orange_r = 0;
manz 5:1da4d4050306 311 brown_r = 0;
manz 5:1da4d4050306 312 yellow_r = 1;
manz 5:1da4d4050306 313
manz 5:1da4d4050306 314 wait_ms(wait_t);
manz 5:1da4d4050306 315 case_right = 0;
manz 5:1da4d4050306 316 break;
manz 5:1da4d4050306 317 }
manz 5:1da4d4050306 318 return 1;
manz 5:1da4d4050306 319 }