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@14:82248fb06e53, 2016-05-12 (annotated)

Committer:
manz
Date:
Thu May 12 06:17:49 2016 +0000
Revision:
14:82248fb06e53
Parent:
7:1bb3b5b66fe8 
latest version;

Who changed what in which revision?

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