Design-in of LPC11U24 (lqfp48) of mbed as Steppermotor controller with USB control.
Mbed repository of Stepper Motor Control board. Using mbed LPC11U24 chip with HID USB.
Hardware in copy repo on bitbucket https://bitbucket.org/jeroen3/stepper-motor-board
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?
User | Revision | Line number | New 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 |