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@0:83acd45a2405, 2016-04-21 (annotated)

Committer:
manz
Date:
Thu Apr 21 22:01:19 2016 +0000
Revision:
0:83acd45a2405
Child:
3:63aef644e6d2
stepper version of SCRIBE

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