Jeroen Lodder / Mbed 2 deprecated SteppermotorBoard

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   USBDevice mbed

SMC.cpp@2:ddae311a4533, 2013-10-30 (annotated)

Committer:
jeroen3
Date:
Wed Oct 30 21:59:32 2013 +0000
Revision:
2:ddae311a4533
Parent:
0:d0306c0cbee6 
fix led blink after soft reset

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jeroen3 0:d0306c0cbee6 1 /**
jeroen3 0:d0306c0cbee6 2 * @file SMC.cpp
jeroen3 0:d0306c0cbee6 3 * @brief Stepper motor control module uses 1/8 microstepping
jeroen3 0:d0306c0cbee6 4 *
jeroen3 0:d0306c0cbee6 5 * @author Jeroen Lodder
jeroen3 0:d0306c0cbee6 6 * @date Oktober 2013
jeroen3 0:d0306c0cbee6 7 *
jeroen3 0:d0306c0cbee6 8 * @{
jeroen3 0:d0306c0cbee6 9 */
jeroen3 0:d0306c0cbee6 10 #include "mbed.h"
jeroen3 0:d0306c0cbee6 11 #include "smc.h"
jeroen3 0:d0306c0cbee6 12 #include "CT32B0_PWM.h"
jeroen3 0:d0306c0cbee6 13
jeroen3 0:d0306c0cbee6 14 #define PWM_PERIOD 513 /**< @brief PWM period integer */
jeroen3 0:d0306c0cbee6 15 #define MIN_STEPTIME_US 100 /**< @brief Minimum time required for one microstep, derived from Hi-side driver latency (in uS) */
jeroen3 0:d0306c0cbee6 16
jeroen3 0:d0306c0cbee6 17 /* Motor Control IO */
jeroen3 0:d0306c0cbee6 18 DigitalOut HIFET_1(P0_16); /**< @brief Hi Side FET 1 of bridge 1 */
jeroen3 0:d0306c0cbee6 19 DigitalOut HIFET_2(P0_17); /**< @brief Hi Side FET 2 of bridge 1 */
jeroen3 0:d0306c0cbee6 20 DigitalOut HIFET_3(P0_18); /**< @brief Hi Side FET 3 of bridge 2 */
jeroen3 0:d0306c0cbee6 21 DigitalOut HIFET_4(P0_19); /**< @brief Hi Side FET 4 of bridge 2 */
jeroen3 0:d0306c0cbee6 22
jeroen3 0:d0306c0cbee6 23 /* Motor Control stepcontrol */
jeroen3 0:d0306c0cbee6 24 Ticker smc;
jeroen3 0:d0306c0cbee6 25 volatile static int smc_walker = 0; /**< @brief Motor control LUT index */
jeroen3 0:d0306c0cbee6 26 volatile static int smc_dir = 1; /**< @brief Director in lookup table (unused) */
jeroen3 0:d0306c0cbee6 27 volatile static int smc_steps = -1; /**< @brief Remaining steps */
jeroen3 0:d0306c0cbee6 28 volatile static int smc_free = 1; /**< @brief Argument to free motor when done */
jeroen3 0:d0306c0cbee6 29 volatile static int smc_abort = 0; /**< @brief Set when motor control should quit by itself */
jeroen3 0:d0306c0cbee6 30 volatile static int smc_steptime = 0; /**< @brief Time used to make 1 step */
jeroen3 0:d0306c0cbee6 31 volatile static int smc_isPaused = 0; /**< @brief Boolean for paused state of control */
jeroen3 0:d0306c0cbee6 32
jeroen3 0:d0306c0cbee6 33 /**
jeroen3 0:d0306c0cbee6 34 * @brief Initializes Stepper Motor Control
jeroen3 0:d0306c0cbee6 35 */
jeroen3 0:d0306c0cbee6 36 void SMC_init(void){
jeroen3 0:d0306c0cbee6 37 // Hi fet low
jeroen3 0:d0306c0cbee6 38 HIFET_1 = 0;
jeroen3 0:d0306c0cbee6 39 HIFET_2 = 0;
jeroen3 0:d0306c0cbee6 40 HIFET_3 = 0;
jeroen3 0:d0306c0cbee6 41 HIFET_4 = 0;
jeroen3 0:d0306c0cbee6 42 // Wait minimum
jeroen3 0:d0306c0cbee6 43
jeroen3 0:d0306c0cbee6 44 // Lo fet low
jeroen3 0:d0306c0cbee6 45 CT32B0_initpwm(PWM_PERIOD,0);
jeroen3 0:d0306c0cbee6 46 // Start pwm
jeroen3 0:d0306c0cbee6 47 CT32B0_start();
jeroen3 0:d0306c0cbee6 48 }
jeroen3 0:d0306c0cbee6 49
jeroen3 0:d0306c0cbee6 50 /**
jeroen3 0:d0306c0cbee6 51 * @brief De-Initializes Stepper Motor Control
jeroen3 0:d0306c0cbee6 52 */
jeroen3 0:d0306c0cbee6 53 void SMC_deinit(void){
jeroen3 0:d0306c0cbee6 54 smc.detach();
jeroen3 0:d0306c0cbee6 55 CT32B0_deinit(0);
jeroen3 0:d0306c0cbee6 56
jeroen3 0:d0306c0cbee6 57 HIFET_1 = 0;
jeroen3 0:d0306c0cbee6 58 HIFET_2 = 0;
jeroen3 0:d0306c0cbee6 59 HIFET_3 = 0;
jeroen3 0:d0306c0cbee6 60 HIFET_4 = 0;
jeroen3 0:d0306c0cbee6 61
jeroen3 0:d0306c0cbee6 62 smc_walker = 0;
jeroen3 0:d0306c0cbee6 63 smc_dir = 1;
jeroen3 0:d0306c0cbee6 64 smc_steps = -1;
jeroen3 0:d0306c0cbee6 65 smc_free = 1;
jeroen3 0:d0306c0cbee6 66 smc_abort = 0;
jeroen3 0:d0306c0cbee6 67 smc_isPaused = 0;
jeroen3 0:d0306c0cbee6 68 }
jeroen3 0:d0306c0cbee6 69
jeroen3 0:d0306c0cbee6 70 /**
jeroen3 0:d0306c0cbee6 71 * @brief Routine called by interrupt to modify H-Bridge states
jeroen3 0:d0306c0cbee6 72 */
jeroen3 0:d0306c0cbee6 73 void SMC_routine(void){
jeroen3 0:d0306c0cbee6 74 #define i smc_walker
jeroen3 0:d0306c0cbee6 75 CT32B0_wait_refresh();
jeroen3 0:d0306c0cbee6 76 __disable_irq();
jeroen3 0:d0306c0cbee6 77 // Phase 1 A
jeroen3 0:d0306c0cbee6 78 // If sin +, H1->L2
jeroen3 0:d0306c0cbee6 79 // If sin -, H2->L1
jeroen3 0:d0306c0cbee6 80 // If direction -1, swap motor A channels, this changes direction.
jeroen3 0:d0306c0cbee6 81 // Reversing lookup table is not effective
jeroen3 0:d0306c0cbee6 82
jeroen3 0:d0306c0cbee6 83 if(smc_dir > 0){
jeroen3 0:d0306c0cbee6 84 HIFET_1 = LUT_H1[i];
jeroen3 0:d0306c0cbee6 85 HIFET_2 = LUT_H2[i];
jeroen3 0:d0306c0cbee6 86 if(LUT_L1[i] == 0)
jeroen3 0:d0306c0cbee6 87 CT32B0_set(0, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 88 else
jeroen3 0:d0306c0cbee6 89 CT32B0_set(0, PWM_PERIOD-LUT_L1[i] );
jeroen3 0:d0306c0cbee6 90 if(LUT_L2[i] == 0)
jeroen3 0:d0306c0cbee6 91 CT32B0_set(1, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 92 else
jeroen3 0:d0306c0cbee6 93 CT32B0_set(1, PWM_PERIOD-LUT_L2[i] );
jeroen3 0:d0306c0cbee6 94 }else{
jeroen3 0:d0306c0cbee6 95 // Reversed for dir -1
jeroen3 0:d0306c0cbee6 96 HIFET_1 = LUT_H2[i];
jeroen3 0:d0306c0cbee6 97 HIFET_2 = LUT_H1[i];
jeroen3 0:d0306c0cbee6 98 if(LUT_L2[i] == 0)
jeroen3 0:d0306c0cbee6 99 CT32B0_set(0, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 100 else
jeroen3 0:d0306c0cbee6 101 CT32B0_set(0, PWM_PERIOD-LUT_L2[i] );
jeroen3 0:d0306c0cbee6 102 if(LUT_L1[i] == 0)
jeroen3 0:d0306c0cbee6 103 CT32B0_set(1, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 104 else
jeroen3 0:d0306c0cbee6 105 CT32B0_set(1, PWM_PERIOD-LUT_L1[i] );
jeroen3 0:d0306c0cbee6 106 }
jeroen3 0:d0306c0cbee6 107
jeroen3 0:d0306c0cbee6 108 // Phase 1 A
jeroen3 0:d0306c0cbee6 109 // If sin +, H1->L2
jeroen3 0:d0306c0cbee6 110 // If sin -, H2->L1
jeroen3 0:d0306c0cbee6 111 HIFET_3 = LUT_H3[i];
jeroen3 0:d0306c0cbee6 112 HIFET_4 = LUT_H4[i];
jeroen3 0:d0306c0cbee6 113 if(LUT_L3[i] == 0)
jeroen3 0:d0306c0cbee6 114 CT32B0_set(2, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 115 else
jeroen3 0:d0306c0cbee6 116 CT32B0_set(2, PWM_PERIOD-LUT_L3[i] );
jeroen3 0:d0306c0cbee6 117 if(LUT_L4[i] == 0)
jeroen3 0:d0306c0cbee6 118 CT32B0_set(3, (PWM_PERIOD+1) );
jeroen3 0:d0306c0cbee6 119 else
jeroen3 0:d0306c0cbee6 120 CT32B0_set(3, PWM_PERIOD-LUT_L4[i] );
jeroen3 0:d0306c0cbee6 121 if(i==9)
jeroen3 0:d0306c0cbee6 122 CT32B0_stage(1);
jeroen3 0:d0306c0cbee6 123 if(i==24)
jeroen3 0:d0306c0cbee6 124 CT32B0_stage(0);
jeroen3 0:d0306c0cbee6 125 CT32B0_reload_mat();
jeroen3 0:d0306c0cbee6 126
jeroen3 0:d0306c0cbee6 127 /*
jeroen3 0:d0306c0cbee6 128 volatile static uint8_t hifet[4] = {0,0,0,0};
jeroen3 0:d0306c0cbee6 129 hifet[0] = HIFET_1;
jeroen3 0:d0306c0cbee6 130 hifet[1] = HIFET_2;
jeroen3 0:d0306c0cbee6 131 hifet[2] = HIFET_3;
jeroen3 0:d0306c0cbee6 132 hifet[3] = HIFET_4;
jeroen3 0:d0306c0cbee6 133 volatile static uint8_t lofet[4] = {0,0,0,0};
jeroen3 0:d0306c0cbee6 134 if(LUT_L1[i]) lofet[0] = 1; else lofet[0] = 0;
jeroen3 0:d0306c0cbee6 135 if(LUT_L2[i]) lofet[1] = 1; else lofet[1] = 0;
jeroen3 0:d0306c0cbee6 136 if(LUT_L3[i]) lofet[2] = 1; else lofet[2] = 0;
jeroen3 0:d0306c0cbee6 137 if(LUT_L4[i]) lofet[3] = 1; else lofet[3] = 0;
jeroen3 0:d0306c0cbee6 138 volatile static uint8_t errfet[4] = {0,0,0,0};
jeroen3 0:d0306c0cbee6 139 errfet[0] = hifet[0] & lofet[0];
jeroen3 0:d0306c0cbee6 140 errfet[1] = hifet[1] & lofet[1];
jeroen3 0:d0306c0cbee6 141 errfet[2] = hifet[2] & lofet[2];
jeroen3 0:d0306c0cbee6 142 errfet[3] = hifet[3] & lofet[3];
jeroen3 0:d0306c0cbee6 143
jeroen3 0:d0306c0cbee6 144 if( errfet[0] | errfet[1] | errfet[2] | errfet[3] ){
jeroen3 0:d0306c0cbee6 145 // ILLEGAL MODE
jeroen3 0:d0306c0cbee6 146 // smc_abort = 1;
jeroen3 0:d0306c0cbee6 147 __NOP();
jeroen3 0:d0306c0cbee6 148 } */
jeroen3 0:d0306c0cbee6 149
jeroen3 0:d0306c0cbee6 150 #undef i
jeroen3 0:d0306c0cbee6 151
jeroen3 0:d0306c0cbee6 152 /* Walk */
jeroen3 0:d0306c0cbee6 153 smc_walker += 1; //abs(smc_dir);
jeroen3 0:d0306c0cbee6 154 if(smc_walker > 31)
jeroen3 0:d0306c0cbee6 155 smc_walker = 0;
jeroen3 0:d0306c0cbee6 156 //if(smc_walker < 0)
jeroen3 0:d0306c0cbee6 157 //smc_walker = 31;
jeroen3 0:d0306c0cbee6 158 /* Coutdown */
jeroen3 0:d0306c0cbee6 159 if(smc_steps != -1){
jeroen3 0:d0306c0cbee6 160 if(smc_steps == 0 || smc_abort == 1){
jeroen3 0:d0306c0cbee6 161 if(smc_free || smc_abort == 1){
jeroen3 0:d0306c0cbee6 162 // motor free
jeroen3 0:d0306c0cbee6 163 HIFET_1 = 0;
jeroen3 0:d0306c0cbee6 164 HIFET_2 = 0;
jeroen3 0:d0306c0cbee6 165 HIFET_3 = 0;
jeroen3 0:d0306c0cbee6 166 HIFET_4 = 0;
jeroen3 0:d0306c0cbee6 167 CT32B0_deinit(0);
jeroen3 0:d0306c0cbee6 168 }else{
jeroen3 0:d0306c0cbee6 169 // motor locked
jeroen3 0:d0306c0cbee6 170 }
jeroen3 0:d0306c0cbee6 171 smc.detach();
jeroen3 0:d0306c0cbee6 172 smc_abort = 0;
jeroen3 0:d0306c0cbee6 173 smc_isPaused = 0;
jeroen3 0:d0306c0cbee6 174 }
jeroen3 0:d0306c0cbee6 175 smc_steps--;
jeroen3 0:d0306c0cbee6 176 }
jeroen3 0:d0306c0cbee6 177 __enable_irq();
jeroen3 0:d0306c0cbee6 178 }
jeroen3 0:d0306c0cbee6 179
jeroen3 0:d0306c0cbee6 180 /**
jeroen3 0:d0306c0cbee6 181 * @brief Stepper motor control main command
jeroen3 0:d0306c0cbee6 182 *
jeroen3 0:d0306c0cbee6 183 * @param[in] steps Number of steps to take
jeroen3 0:d0306c0cbee6 184 * @param[in] dir Direction to step in, 1 or 0
jeroen3 0:d0306c0cbee6 185 * @param[in] time_ms Time to take for these steps
jeroen3 0:d0306c0cbee6 186 * @param[in] free Free or lock motor when done 1 or 0 respectively
jeroen3 0:d0306c0cbee6 187 * @return -1 when illegal command or mode
jeroen3 0:d0306c0cbee6 188 */
jeroen3 0:d0306c0cbee6 189 int SMC_step(int steps, uint8_t dir, uint32_t time_ms, uint8_t free){
jeroen3 0:d0306c0cbee6 190 // steps = number of microsteps (8 microsteps per full step)
jeroen3 0:d0306c0cbee6 191 // dir = -1 or 1
jeroen3 0:d0306c0cbee6 192 // time_us = completion time in s
jeroen3 0:d0306c0cbee6 193 uint32_t steptime = (time_ms*1000)/steps;
jeroen3 0:d0306c0cbee6 194 // Parameter check
jeroen3 0:d0306c0cbee6 195 if(smc_steps != -1) return -1; // Only if motor idle
jeroen3 0:d0306c0cbee6 196 if(steps < 1) return -1; // At least one step
jeroen3 0:d0306c0cbee6 197 if(dir) dir = 1; // Round dir to bool
jeroen3 0:d0306c0cbee6 198 if(free) free = 1; // Round free to bool
jeroen3 0:d0306c0cbee6 199 if(steptime < MIN_STEPTIME_US) // Verify steptime
jeroen3 0:d0306c0cbee6 200 return -1;
jeroen3 0:d0306c0cbee6 201 if(dir == 0)
jeroen3 0:d0306c0cbee6 202 smc_dir = -1;
jeroen3 0:d0306c0cbee6 203 else
jeroen3 0:d0306c0cbee6 204 smc_dir = 1;
jeroen3 0:d0306c0cbee6 205 // Configure stepper
jeroen3 0:d0306c0cbee6 206 smc_steps = steps;
jeroen3 0:d0306c0cbee6 207 smc_free = free;
jeroen3 0:d0306c0cbee6 208 smc_steptime = steptime;
jeroen3 0:d0306c0cbee6 209 smc_isPaused = 0;
jeroen3 0:d0306c0cbee6 210 // Initiate
jeroen3 0:d0306c0cbee6 211 SMC_init();
jeroen3 0:d0306c0cbee6 212 smc.attach_us(&SMC_routine, smc_steptime);
jeroen3 0:d0306c0cbee6 213 return 0;
jeroen3 0:d0306c0cbee6 214 }
jeroen3 0:d0306c0cbee6 215
jeroen3 0:d0306c0cbee6 216 /**
jeroen3 0:d0306c0cbee6 217 * @brief Return 1 of stepper motor control is idle
jeroen3 0:d0306c0cbee6 218 */
jeroen3 0:d0306c0cbee6 219 uint32_t SMC_idle(void){
jeroen3 0:d0306c0cbee6 220 if(smc_steps == -1)
jeroen3 0:d0306c0cbee6 221 return 1;
jeroen3 0:d0306c0cbee6 222 else
jeroen3 0:d0306c0cbee6 223 return 0;
jeroen3 0:d0306c0cbee6 224 }
jeroen3 0:d0306c0cbee6 225
jeroen3 0:d0306c0cbee6 226 /**
jeroen3 0:d0306c0cbee6 227 * @brief Puts motor in brake mode, enable all low-side mosfets
jeroen3 0:d0306c0cbee6 228 */
jeroen3 0:d0306c0cbee6 229 void SMC_brake(void){
jeroen3 0:d0306c0cbee6 230 // Do not brake when active control
jeroen3 0:d0306c0cbee6 231 if(smc_walker > 0)
jeroen3 0:d0306c0cbee6 232 return;
jeroen3 0:d0306c0cbee6 233 // Regular deinit
jeroen3 0:d0306c0cbee6 234 SMC_deinit();
jeroen3 0:d0306c0cbee6 235 // Re-enable low side
jeroen3 0:d0306c0cbee6 236 CT32B0_deinit(1);
jeroen3 0:d0306c0cbee6 237 }
jeroen3 0:d0306c0cbee6 238
jeroen3 0:d0306c0cbee6 239 /**
jeroen3 0:d0306c0cbee6 240 * @brief Puts motor in free mode, disables all mosfets
jeroen3 0:d0306c0cbee6 241 */
jeroen3 0:d0306c0cbee6 242 void SMC_free(void){
jeroen3 0:d0306c0cbee6 243 // Do not free when active control
jeroen3 0:d0306c0cbee6 244 if(smc_walker > 0)
jeroen3 0:d0306c0cbee6 245 return;
jeroen3 0:d0306c0cbee6 246 // Regular deinit
jeroen3 0:d0306c0cbee6 247 SMC_deinit();
jeroen3 0:d0306c0cbee6 248 // Re-enable low side
jeroen3 0:d0306c0cbee6 249 CT32B0_deinit(0);
jeroen3 0:d0306c0cbee6 250 }
jeroen3 0:d0306c0cbee6 251
jeroen3 0:d0306c0cbee6 252 /**
jeroen3 0:d0306c0cbee6 253 * @brief Pause current stepping command
jeroen3 0:d0306c0cbee6 254 */
jeroen3 0:d0306c0cbee6 255 void SMC_pause(void){
jeroen3 0:d0306c0cbee6 256 if( !SMC_idle() ){
jeroen3 0:d0306c0cbee6 257 smc.detach();
jeroen3 0:d0306c0cbee6 258 smc_isPaused = 1;
jeroen3 0:d0306c0cbee6 259 }
jeroen3 0:d0306c0cbee6 260 }
jeroen3 0:d0306c0cbee6 261
jeroen3 0:d0306c0cbee6 262 /**
jeroen3 0:d0306c0cbee6 263 * @brief Continue from pause
jeroen3 0:d0306c0cbee6 264 */
jeroen3 0:d0306c0cbee6 265 void SMC_continue(void){
jeroen3 0:d0306c0cbee6 266 if( !SMC_idle() && smc_isPaused==1 ){
jeroen3 0:d0306c0cbee6 267 smc.attach_us(&SMC_routine, smc_steptime);
jeroen3 0:d0306c0cbee6 268 smc_isPaused = 0;
jeroen3 0:d0306c0cbee6 269 }
jeroen3 0:d0306c0cbee6 270 }
jeroen3 0:d0306c0cbee6 271
jeroen3 0:d0306c0cbee6 272 /**
jeroen3 0:d0306c0cbee6 273 * @brief Return remaining steps, negative if paused
jeroen3 0:d0306c0cbee6 274 */
jeroen3 0:d0306c0cbee6 275 int SMC_getState(void){
jeroen3 0:d0306c0cbee6 276 if( smc_steps < 0 ){
jeroen3 0:d0306c0cbee6 277 return 0;
jeroen3 0:d0306c0cbee6 278 }else{
jeroen3 0:d0306c0cbee6 279 if(smc_isPaused)
jeroen3 0:d0306c0cbee6 280 return (-1*smc_steps);
jeroen3 0:d0306c0cbee6 281 else
jeroen3 0:d0306c0cbee6 282 return (1*smc_steps);
jeroen3 0:d0306c0cbee6 283 }
jeroen3 0:d0306c0cbee6 284 }
jeroen3 0:d0306c0cbee6 285
jeroen3 0:d0306c0cbee6 286 /**
jeroen3 0:d0306c0cbee6 287 * @brief Egg, open it to find out
jeroen3 0:d0306c0cbee6 288 */
jeroen3 0:d0306c0cbee6 289 void SMC_egg(void){
jeroen3 0:d0306c0cbee6 290 const uint16_t rr[] = {
jeroen3 0:d0306c0cbee6 291 627 , 1045 ,
jeroen3 0:d0306c0cbee6 292 122 , 785 ,
jeroen3 0:d0306c0cbee6 293 887 , 187 ,
jeroen3 0:d0306c0cbee6 294 233 , 166 ,
jeroen3 0:d0306c0cbee6 295 788 , 752 ,
jeroen3 0:d0306c0cbee6 296 1157
jeroen3 0:d0306c0cbee6 297 };
jeroen3 0:d0306c0cbee6 298 int rri=0;
jeroen3 0:d0306c0cbee6 299 // Egg
jeroen3 0:d0306c0cbee6 300 for(int i=0;i<2; i++){
jeroen3 0:d0306c0cbee6 301 SMC_step(rr[rri++] , 1, 685, 1); // 83 - 987
jeroen3 0:d0306c0cbee6 302 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 303 wait_ms(1);
jeroen3 0:d0306c0cbee6 304 SMC_step(rr[rri++] , 1, 965, 1); // 85 - 1108
jeroen3 0:d0306c0cbee6 305 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 306 wait_ms(1);
jeroen3 0:d0306c0cbee6 307 SMC_step(rr[rri++] , 1, 245, 1); // 78 - 739
jeroen3 0:d0306c0cbee6 308 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 309 wait_ms(1);
jeroen3 0:d0306c0cbee6 310 SMC_step(rr[rri++] , 1, 725, 1); // 85 - 1108
jeroen3 0:d0306c0cbee6 311 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 312 wait_ms(1);
jeroen3 0:d0306c0cbee6 313 SMC_step(rr[rri++] , 1, 710, 1); // 87 - 1244
jeroen3 0:d0306c0cbee6 314 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 315 wait_ms(1);
jeroen3 0:d0306c0cbee6 316 SMC_step(rr[rri++] , 1, 125, 1); // 90 - 1479
jeroen3 0:d0306c0cbee6 317 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 318 wait_ms(1);
jeroen3 0:d0306c0cbee6 319 SMC_step(rr[rri++] , 1, 175, 1); // 88 - 1318
jeroen3 0:d0306c0cbee6 320 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 321 wait_ms(1);
jeroen3 0:d0306c0cbee6 322 SMC_step(rr[rri++] , 1, 133, 1); // 87 - 1244
jeroen3 0:d0306c0cbee6 323 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 324 wait_ms(1);
jeroen3 0:d0306c0cbee6 325 SMC_step(rr[rri++] , 1, 860, 1); // 83 - 987
jeroen3 0:d0306c0cbee6 326 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 327 wait_ms(1);
jeroen3 0:d0306c0cbee6 328 SMC_step(rr[rri++] , 1, 695, 1); // 85 - 1108
jeroen3 0:d0306c0cbee6 329 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 330 wait_ms(1);
jeroen3 0:d0306c0cbee6 331 SMC_step(rr[rri++] , 1, 2315, 1); // 78 - 739
jeroen3 0:d0306c0cbee6 332 while( !SMC_idle()) ; // -
jeroen3 0:d0306c0cbee6 333 wait_ms(1);
jeroen3 0:d0306c0cbee6 334 rri=0;
jeroen3 0:d0306c0cbee6 335 }
jeroen3 0:d0306c0cbee6 336 }
jeroen3 0:d0306c0cbee6 337 /**
jeroen3 0:d0306c0cbee6 338 *@}
jeroen3 0:d0306c0cbee6 339 */
jeroen3 0:d0306c0cbee6 340
jeroen3 0:d0306c0cbee6 341