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@6:ca112c3083bb, 2016-05-12 (annotated)

Committer:
manz
Date:
Thu May 12 06:12:45 2016 +0000
Revision:
6:ca112c3083bb
Parent:
5:2c196f871096 
latest version;

Who changed what in which revision?

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